机械臂的路径规划

路径规划基本概念

机械臂视觉操作流程 = 手眼标定 + 目标识别与定位 + 操作姿态分析 + 运动规划 = 手眼标定 + 深度学习 + 运动规划

手眼标定

作用: 获得机器人坐标系和相机坐标系的关系
效果: 目前可达到误差在土5mm

运动规划

作用: 到达算法指定位置和姿态
路径规划: 从当前位置到期望位置规划一条无碰撞轨迹

路径规划在机械臂领域所处的位置–框架设计

• 视觉：相机数据–》点云生成–》姿态预测–》手眼标定
• 规划：碰撞检测 路径规划
• 控制：上位机通讯 执行轨迹

• 区分路径规划、轨迹规划与运动规划

路径规划(path planning)：规划的是路径点，即机器人从起始点运动到目标点的路线，它规划的结果不包含时间信息。

轨迹规划(trajectory planning)：规划的是轨迹，即机器人何时、以何种速度到达某路径点，如机械臂各关节角度随时间变化的关系，它规划的结果是包含时间信息的。

运动规划(motion planning)：概念更加宽泛，可以近似理解为等于路径规划+轨迹规划。

算法类型	具体算法	是否完备	是否最优
基于搜索	Dijkstra、A*	概率完备	是
基于采样	PRM、RRT、RRT-Connect	概率完备	否
	RRT、RRT-smart		渐进最优
基于智能算法	遗传算法、蚁群算法	概率完备	否

• 完备性:是指如果在起始点和目标点间有路径解存在，那么一定可以得到解，如果得不到解那么一定说明没有解存在:
• 概率完备性:是指如果在起始点和目标点间有路径解存在，只要规划或搜索的时间足够长，就一定能确保找到一条路径解;
• 最优性:是指规划得到的路径在某个评价指标上是最优的;
• 渐进最优性:是指经过有限次规划迭代后得到的路径是接近最优的次优路径，且每次迭代后都与最优路径更加接近，是一个逐渐收敛的过程;

• (一)笛卡尔空间
  (1)直线运动 (2)圆运动
• (二)关节空间
  (1)点到点
  1.三次/五次多项式
  2.T型曲线
  3.S型曲线
  (2)路径点
  1.三次/五次样条
  2.过路径点的T型曲线
  3.贝塞尔曲线
  4.B样条(贝塞尔曲线改进)
  5.NURBS曲线
  6.基于优化的算法如minimum jerk/snap

RRT算法精讲

基于采样的路径规划算法简介

• PRM: 1.先随机采样n个点构成图 2.基于图进行搜索
• RRT: 快速搜索随机树，非最优解; 查询near可用kdtree等数据结构，快速找到树中最近结点
• RRT*: 作了两个改进，一是改变了新节点连接到树的规则，二是对搜索树进行“剪枝”操作，使之更接近于真实的最优路径
• RRT-connect: 双向扩展的RRT，从start和goal同时扩展，搜索速度比较快，ROS-Moveit!默认使用此算法

import copy
import math
import random
import time
 
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation as Rot
import numpy as np
 
show_animation = True
 
 
class RRT:
 
    # randArea采样范围[-2--18] obstacleList设置的障碍物 maxIter最大迭代次数 expandDis采样步长为2.0 goalSampleRate 以10%的概率将终点作为采样点
    def __init__(self, obstacleList, randArea,
                 expandDis=2.0, goalSampleRate=10, maxIter=200):
 
        self.start = None
        self.goal = None
        self.min_rand = randArea[0]
        self.max_rand = randArea[1]
        self.expand_dis = expandDis
        self.goal_sample_rate = goalSampleRate
        self.max_iter = maxIter
        self.obstacle_list = obstacleList
        # 存储RRT树
        self.node_list = None
 
    # start、goal 起点终点坐标
    def rrt_planning(self, start, goal, animation=True):
        start_time = time.time()
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        # 将起点加入node_list作为树的根结点
        self.node_list = [self.start]
        path = None
 
        for i in range(self.max_iter):
            # 进行采样
            rnd = self.sample()
            # 取的距离采样点最近的节点下标
            n_ind = self.get_nearest_list_index(self.node_list, rnd)
            # 得到最近节点
            nearestNode = self.node_list[n_ind]
 
            # 将Xrandom和Xnear连线方向作为生长方向
            # math.atan2() 函数接受两个参数，分别是 y 坐标差值和 x 坐标差值。它返回的值是以弧度表示的角度，范围在 -π 到 π 之间。这个角度表示了从 nearestNode 指向 rnd 的方向。
            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
            # 生长 ： 输入参数为角度、下标、nodelist中最近的节点  得到生长过后的节点
            newNode = self.get_new_node(theta, n_ind, nearestNode)
            # 检查是否有障碍物 传入参数为新生城路径的两个节点
            noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)
            if noCollision:
                # 没有碰撞把新节点加入到树里面
                self.node_list.append(newNode)
                if animation:
                    self.draw_graph(newNode, path)
 
                # 是否到终点附近
                if self.is_near_goal(newNode):
                    # 是否这条路径与障碍物发生碰撞
                    if self.check_segment_collision(newNode.x, newNode.y,
                                                    self.goal.x, self.goal.y):
                        lastIndex = len(self.node_list) - 1
                        # 找路径
                        path = self.get_final_course(lastIndex)
                        pathLen = self.get_path_len(path)
                        print("current path length: {}, It costs {} s".format(pathLen, time.time()-start_time))
 
                        if animation:
                            self.draw_graph(newNode, path)
                        return path
 
    def rrt_star_planning(self, start, goal, animation=True):
        start_time = time.time()
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.node_list = [self.start]
        path = None
        lastPathLength = float('inf')
 
        for i in range(self.max_iter):
            rnd = self.sample()
            n_ind = self.get_nearest_list_index(self.node_list, rnd)
            nearestNode = self.node_list[n_ind]
 
            # steer
            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
            newNode = self.get_new_node(theta, n_ind, nearestNode)
 
            noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)
            if noCollision:
                nearInds = self.find_near_nodes(newNode)
                newNode = self.choose_parent(newNode, nearInds)
 
                self.node_list.append(newNode)
                self.rewire(newNode, nearInds)
 
                if animation:
                    self.draw_graph(newNode, path)
 
                if self.is_near_goal(newNode):
                    if self.check_segment_collision(newNode.x, newNode.y,
                                                    self.goal.x, self.goal.y):
                        lastIndex = len(self.node_list) - 1
 
                        tempPath = self.get_final_course(lastIndex)
                        tempPathLen = self.get_path_len(tempPath)
                        if lastPathLength > tempPathLen:
                            path = tempPath
                            lastPathLength = tempPathLen
                            print("current path length: {}, It costs {} s".format(tempPathLen, time.time()-start_time))
 
        return path
 
    def informed_rrt_star_planning(self, start, goal, animation=True):
        start_time = time.time()
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.node_list = [self.start]
        # max length we expect to find in our 'informed' sample space,
        # starts as infinite
        cBest = float('inf')
        path = None
 
        # Computing the sampling space
        cMin = math.sqrt(pow(self.start.x - self.goal.x, 2)
                         + pow(self.start.y - self.goal.y, 2))
        xCenter = np.array([[(self.start.x + self.goal.x) / 2.0],
                            [(self.start.y + self.goal.y) / 2.0], [0]])
        a1 = np.array([[(self.goal.x - self.start.x) / cMin],
                       [(self.goal.y - self.start.y) / cMin], [0]])
 
        e_theta = math.atan2(a1[1], a1[0])
 
        # 论文方法求旋转矩阵（2选1）
        # first column of identity matrix transposed
        # id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
        # M = a1 @ id1_t
        # U, S, Vh = np.linalg.svd(M, True, True)
        # C = np.dot(np.dot(U, np.diag(
        #     [1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])),
        #            Vh)
 
        # 直接用二维平面上的公式（2选1）
        C = np.array([[math.cos(e_theta), -math.sin(e_theta), 0],
                      [math.sin(e_theta), math.cos(e_theta),  0],
                      [0,                 0,                  1]])
 
        for i in range(self.max_iter):
            # Sample space is defined by cBest
            # cMin is the minimum distance between the start point and the goal
            # xCenter is the midpoint between the start and the goal
            # cBest changes when a new path is found
 
            rnd = self.informed_sample(cBest, cMin, xCenter, C)
            n_ind = self.get_nearest_list_index(self.node_list, rnd)
            nearestNode = self.node_list[n_ind]
 
            # steer
            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
            newNode = self.get_new_node(theta, n_ind, nearestNode)
 
            noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)
            if noCollision:
                nearInds = self.find_near_nodes(newNode)
                newNode = self.choose_parent(newNode, nearInds)
 
                self.node_list.append(newNode)
                self.rewire(newNode, nearInds)
 
                if self.is_near_goal(newNode):
                    if self.check_segment_collision(newNode.x, newNode.y,
                                                    self.goal.x, self.goal.y):
                        lastIndex = len(self.node_list) - 1
                        tempPath = self.get_final_course(lastIndex)
                        tempPathLen = self.get_path_len(tempPath)
                        if tempPathLen < cBest:
                            path = tempPath
                            cBest = tempPathLen
                            print("current path length: {}, It costs {} s".format(tempPathLen, time.time()-start_time))
            if animation:
                self.draw_graph_informed_RRTStar(xCenter=xCenter,
                                                cBest=cBest, cMin=cMin,
                                                e_theta=e_theta, rnd=rnd, path=path)
 
        return path
 
    def sample(self):
        # 取得1-100的随机数,如果比10大的话（以10%的概率取到终点）
        if random.randint(0, 100) > self.goal_sample_rate:
            # 在空间里随机采样一个点
            rnd = [random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)]
        else:  # goal point sampling
            # 终点作为采样点
            rnd = [self.goal.x, self.goal.y]
        return rnd
 
    def choose_parent(self, newNode, nearInds):
        if len(nearInds) == 0:
            return newNode
 
        dList = []
        for i in nearInds:
            dx = newNode.x - self.node_list[i].x
            dy = newNode.y - self.node_list[i].y
            d = math.hypot(dx, dy)
            theta = math.atan2(dy, dx)
            if self.check_collision(self.node_list[i], theta, d):
                dList.append(self.node_list[i].cost + d)
            else:
                dList.append(float('inf'))
 
        minCost = min(dList)
        minInd = nearInds[dList.index(minCost)]
 
        if minCost == float('inf'):
            print("min cost is inf")
            return newNode
 
        newNode.cost = minCost
        newNode.parent = minInd
 
        return newNode
 
    def find_near_nodes(self, newNode):
        n_node = len(self.node_list)
        r = 50.0 * math.sqrt((math.log(n_node) / n_node))
        d_list = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2
                  for node in self.node_list]
        near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]
        return near_inds
 
    def informed_sample(self, cMax, cMin, xCenter, C):
        if cMax < float('inf'):
            r = [cMax / 2.0,
                 math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,
                 math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]
            L = np.diag(r)
            xBall = self.sample_unit_ball()
            rnd = np.dot(np.dot(C, L), xBall) + xCenter
            rnd = [rnd[(0, 0)], rnd[(1, 0)]]
        else:
            rnd = self.sample()
 
        return rnd
 
    @staticmethod
    def sample_unit_ball():
        a = random.random()
        b = random.random()
 
        if b < a:
            a, b = b, a
 
        sample = (b * math.cos(2 * math.pi * a / b),
                  b * math.sin(2 * math.pi * a / b))
        return np.array([[sample[0]], [sample[1]], [0]])
 
    @staticmethod
    def get_path_len(path):
        pathLen = 0
        for i in range(1, len(path)):
            node1_x = path[i][0]
            node1_y = path[i][1]
            node2_x = path[i - 1][0]
            node2_y = path[i - 1][1]
            pathLen += math.sqrt((node1_x - node2_x)
                                 ** 2 + (node1_y - node2_y) ** 2)
 
        return pathLen
 
    @staticmethod
    def line_cost(node1, node2):
        return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)
 
    @staticmethod
    def get_nearest_list_index(nodes, rnd):
        # 遍历所有节点 计算采样点和节点的距离
        dList = [(node.x - rnd[0]) ** 2
                 + (node.y - rnd[1]) ** 2 for node in nodes]
        # 获得最近的距离所对应的索引
        minIndex = dList.index(min(dList))
        return minIndex
 
    def get_new_node(self, theta, n_ind, nearestNode):
        newNode = copy.deepcopy(nearestNode)
 
        # 坐标
        newNode.x += self.expand_dis * math.cos(theta)
        newNode.y += self.expand_dis * math.sin(theta)
        # 代价
        newNode.cost += self.expand_dis
        # 父亲节点
        newNode.parent = n_ind
        return newNode
 
    def is_near_goal(self, node):
        # 计算距离
        d = self.line_cost(node, self.goal)
        if d < self.expand_dis:
            return True
        return False
 
    def rewire(self, newNode, nearInds):
        n_node = len(self.node_list)
        for i in nearInds:
            nearNode = self.node_list[i]
 
            d = math.sqrt((nearNode.x - newNode.x) ** 2
                          + (nearNode.y - newNode.y) ** 2)
 
            s_cost = newNode.cost + d
 
            if nearNode.cost > s_cost:
                theta = math.atan2(newNode.y - nearNode.y,
                                   newNode.x - nearNode.x)
                if self.check_collision(nearNode, theta, d):
                    nearNode.parent = n_node - 1
                    nearNode.cost = s_cost
 
    @staticmethod
    def distance_squared_point_to_segment(v, w, p):
        # Return minimum distance between line segment vw and point p
        if np.array_equal(v, w):
            return (p - v).dot(p - v)  # v == w case
        l2 = (w - v).dot(w - v)  # i.e. |w-v|^2 -  avoid a sqrt
        # Consider the line extending the segment,
        # parameterized as v + t (w - v).
        # We find projection of point p onto the line.
        # It falls where t = [(p-v) . (w-v)] / |w-v|^2
        # We clamp t from [0,1] to handle points outside the segment vw.
        t = max(0, min(1, (p - v).dot(w - v) / l2))
        projection = v + t * (w - v)  # Projection falls on the segment
        return (p - projection).dot(p - projection)
 
    def check_segment_collision(self, x1, y1, x2, y2):
        # 遍历所有的障碍物
        for (ox, oy, size) in self.obstacle_list:
            dd = self.distance_squared_point_to_segment(
                np.array([x1, y1]),
                np.array([x2, y2]),
                np.array([ox, oy]))
            if dd <= size ** 2:
                return False  # collision
        return True
 
    def check_collision(self, nearNode, theta, d):
        tmpNode = copy.deepcopy(nearNode)
        end_x = tmpNode.x + math.cos(theta) * d
        end_y = tmpNode.y + math.sin(theta) * d
        return self.check_segment_collision(tmpNode.x, tmpNode.y, end_x, end_y)
 
    def get_final_course(self, lastIndex):
        path = [[self.goal.x, self.goal.y]]
        while self.node_list[lastIndex].parent is not None:
            node = self.node_list[lastIndex]
            path.append([node.x, node.y])
            lastIndex = node.parent
        path.append([self.start.x, self.start.y])
        return path
 
    def draw_graph_informed_RRTStar(self, xCenter=None, cBest=None, cMin=None, e_theta=None, rnd=None, path=None):
        plt.clf()
        # for stopping simulation with the esc key.
        plt.gcf().canvas.mpl_connect(
            'key_release_event',
            lambda event: [exit(0) if event.key == 'escape' else None])
        if rnd is not None:
            plt.plot(rnd[0], rnd[1], "^k")
            if cBest != float('inf'):
                self.plot_ellipse(xCenter, cBest, cMin, e_theta)
 
        for node in self.node_list:
            if node.parent is not None:
                if node.x or node.y is not None:
                    plt.plot([node.x, self.node_list[node.parent].x], [
                        node.y, self.node_list[node.parent].y], "-g")
 
        for (ox, oy, size) in self.obstacle_list:
            plt.plot(ox, oy, "ok", ms=30 * size)
 
        if path is not None:
            plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
 
        plt.plot(self.start.x, self.start.y, "xr")
        plt.plot(self.goal.x, self.goal.y, "xr")
        plt.axis([-2, 18, -2, 15])
        plt.grid(True)
        plt.pause(0.01)
 
    @staticmethod
    def plot_ellipse(xCenter, cBest, cMin, e_theta):  # pragma: no cover
 
        a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0
        b = cBest / 2.0
        angle = math.pi / 2.0 - e_theta
        cx = xCenter[0]
        cy = xCenter[1]
        t = np.arange(0, 2 * math.pi + 0.1, 0.1)
        x = [a * math.cos(it) for it in t]
        y = [b * math.sin(it) for it in t]
        rot = Rot.from_euler('z', -angle).as_matrix()[0:2, 0:2]
        fx = rot @ np.array([x, y])
        px = np.array(fx[0, :] + cx).flatten()
        py = np.array(fx[1, :] + cy).flatten()
        plt.plot(cx, cy, "xc")
        plt.plot(px, py, "--c")
 
    def draw_graph(self, rnd=None, path=None):
        plt.clf()
        # for stopping simulation with the esc key.
        plt.gcf().canvas.mpl_connect(
            'key_release_event',
            lambda event: [exit(0) if event.key == 'escape' else None])
        if rnd is not None:
            plt.plot(rnd.x, rnd.y, "^k")
 
        for node in self.node_list:
            if node.parent is not None:
                if node.x or node.y is not None:
                    plt.plot([node.x, self.node_list[node.parent].x], [
                        node.y, self.node_list[node.parent].y], "-g")
 
        for (ox, oy, size) in self.obstacle_list:
            # self.plot_circle(ox, oy, size)
            plt.plot(ox, oy, "ok", ms=30 * size)
 
        plt.plot(self.start.x, self.start.y, "xr")
        plt.plot(self.goal.x, self.goal.y, "xr")
 
        if path is not None:
            plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
 
        plt.axis([-2, 18, -2, 15])
        plt.grid(True)
        plt.pause(0.01)
 
 
class Node:
 
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.cost = 0.0
        self.parent = None
 
 
def main():
    print("Start rrt planning")
 
    # create obstacles
    # obstacleList = [
    #     (3,  3,  1.5),
    #     (12, 2,  3),
    #     (3,  9,  2),
    #     (9,  11, 2),
    # ]
 
    # 设置障碍物 (圆点、半径)
    obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),
                    (9, 5, 2), (8, 10, 1)]
 
    # Set params
    # 采样范围 设置的障碍物 最大迭代次数
    rrt = RRT(randArea=[-2, 18], obstacleList=obstacleList, maxIter=200)
    # 传入的是起点和终点
    path = rrt.rrt_planning(start=[0, 0], goal=[15, 12], animation=show_animation)
    # path = rrt.rrt_star_planning(start=[0, 0], goal=[15, 12], animation=show_animation)
    # path = rrt.informed_rrt_star_planning(start=[0, 0], goal=[15, 12], animation=show_animation)
    print("Done!!")
 
    if show_animation and path:
        plt.show()
 
 
if __name__ == '__main__':
    main()

RRT与RRTStar算法及其matlab实现

地图创建

RRT与RRTStar所用到的地图是相同的。

function [start,target,max_x,max_y,wall,resolution]=creat_map_ob()%创建障碍物地图
%创建地图
clear all;
resolution = 1;%每个格子的长度
 
%map的边界
max_x = 50;
max_y = 50;
 
%obstacle
obstacle(1,:) = [25,5,10,5];
obstacle(2,:) = [15,25,5,25];
obstacle(3,:) = [40,20,5,10];
obstacle(4,:) = [30,25,5,10];
obstacle(5,:) = [5,0,5,20];
figure;%创建新窗口
axis([0 max_x 0 max_y]);
set(gca,'XTick',0:resolution:max_x);%x轴的间隔为1
set(gca,'YTick',0:resolution:max_y);%y轴的间隔为1
grid on;
title('RRT');
xlabel('x');
ylabel('y');
hold on;
 
wall = [obstacle];
for i = 1:1:size(wall,1)
    fill([wall(i,1),wall(i,1) + wall(i,3),wall(i,1) + wall(i,3),wall(i,1)],...
        [wall(i,2),wall(i,2),wall(i,2) + wall(i,4),wall(i,2) + wall(i,4)],'k');
end
pause(1);%延时一秒
%初始点
h=msgbox('初始位置标记！');%弹出初始框提示标记初始位置
uiwait(h,3);%3s后关闭对话框
if ishandle(h) == 1%删除对话框
    delete(h);
end
xlabel('请设置初始点X轴！ ','Color','black');%设置x轴
but = 0;
while(but ~= 1)%收到左键点击
    [xval,yval,but] = ginput(1);
    xval=floor(xval);
    yval=floor(yval);
end
start.x = xval;%初始位置
start.y = yval;
plot(xval,yval,'b^','MarkerFaceColor','b','MarkerSize',10*resolution);%绘制初始点
text(xval + 1,yval + 0.5,'Start');
pause(1);%延时一秒
%目标点
h=msgbox('目标位置标记！');%弹出目标提示标记目标位置
uiwait(h,3);%3s后关闭对话框
if ishandle(h) == 1%删除对话框
    delete(h);
end
xlabel('请设置目标点X轴！ ','Color','black');%设置x轴
but1 = 0;
while(but1 ~= 1)%收到左键点击
    [xval,yval,but1] = ginput(1);
    xval=floor(xval);
    yval=floor(yval);
end
target.x = xval;%目标位置
target.y = yval;
plot(xval,yval,'m^','MarkerFaceColor','m','MarkerSize',10*resolution);%绘制目标点
text(xval + 1,yval + 0.5,'Target');
hold on;

RRT主代码

function RRT()
clc
clear all
close all;
[start,target,max_x,max_y,wall,resolution] = creat_map_ob();
figure;%创建新窗口
axis([0 max_x 0 max_y]);
set(gca,'XTick',0:resolution:max_x);%x轴的间隔为1
set(gca,'YTick',0:resolution:max_y);%y轴的间隔为1
grid on;
title('RRT');
xlabel('x');
ylabel('y');
hold on;
for i = 1:1:size(wall,1)
    fill([wall(i,1),wall(i,1) + wall(i,3),wall(i,1) + wall(i,3),wall(i,1)],...
        [wall(i,2),wall(i,2),wall(i,2) + wall(i,4),wall(i,2) + wall(i,4)],'k');
end
plot(start.x,start.y,'b^','MarkerFaceColor','b','MarkerSize',20*resolution);%绘制初始点
text(start.x + 1,start.y + 0.5,'Start');
plot(target.x,target.y,'m^','MarkerFaceColor','m','MarkerSize',20*resolution);%绘制目标点
text(target.x + 1,target.y + 0.5,'Target');
%初始化随机树
tree.child = [];%存储所有节点
tree.parent = [];%存储节点的父节点
tree.distance = [];%存储child到起点的距离
 
tree.child = start;
tree.parent = start;
tree.distance = 0;
 
%存储新衍生的节点，起点初始化
%target_distance用于存储new_node到终点的距离
new_node.x = start.x;
new_node.y = start.y;
 
target_distance = sqrt((target.x-new_node.x)^2+(target.y-new_node.y)^2);
grow_distance = 1;
target_radius = 1.5;
while target_distance > target_radius
    random_point.x = max_x*rand();
    random_point.y = max_y*rand();
 
    handle1 = plot(random_point.x,random_point.y,'p','MarkerEdgeColor',[0.9290 0.6940 0.1250],'MarkerFaceColor',[0.9290 0.6940 0.1250],'MarkerSize',10*resolution);
    [angle,min_idx] = find_closet_node(random_point.x,random_point.y,tree);
    pause(0.001);
    handle2 = plot([tree.child(min_idx).x,random_point.x],[tree.child(min_idx).y,random_point.y],'-','Color',[0.7 0.7 0.7],'LineWidth',0.8*resolution); 
    %根据步长选择新节点
    pause(0.001);
    new_node.x = tree.child(min_idx).x + grow_distance*cos(angle);
    new_node.y = tree.child(min_idx).y + grow_distance*sin(angle);
    handle3 = plot(new_node.x,new_node.y,'.r','MarkerFaceColor','r','MarkerSize',10*resolution);
    obflag = 1;
    step = 0.01;
    %判断路径是否位于障碍物之中
    if new_node.x < tree.child(min_idx).x
        step = -step;
    end
    for k = 1:1:size(wall,1)
        for i = tree.child(min_idx).x:step:new_node.x
            if angle>pi/2-5e-03 && angle<pi/2+5e-03
                j = tree.child(min_idx).y+i-tree.child(min_idx).x;
            elseif angle>-pi/2-5e-03 && angle<-pi/2+5e-03
                j = tree.child(min_idx).y-i+tree.child(min_idx).x;
            else
                j = tree.child(min_idx).y+(i-tree.child(min_idx).x)*tan(angle);
            end
            if i >= wall(k,1) - 0.005 && i <= (wall(k,1)+wall(k,3)) + 0.005
                if j >= wall(k,2) - 0.005 && j <= (wall(k,2) + wall(k,4)) + 0.005
                    obflag = 0;
                    break
                end
            end
        end
        if obflag == 0
            break
        end
    end
    %更新
    pause(0.001);
    if obflag == 1
        tree.child(end + 1) = new_node;
        tree.parent(end + 1) = tree.child(min_idx);
        tree.distance(end + 1) = 1 + tree.distance(min_idx);
        target_distance = sqrt((target.x-new_node.x)^2 + (target.y-new_node.y)^2);
        %删除红色点
        delete(handle3);
        plot(new_node.x,new_node.y,'.g','MarkerFaceColor','g','MarkerSize',10*resolution); % draw the new node
        %画新连接线
        plot([tree.child(min_idx).x,new_node.x],[tree.child(min_idx).y,new_node.y],'-k','LineWidth',0.8*resolution);
        
    end
    pause(0.001);
    %删除随机点
    delete(handle1);
    %删除最近点与随机点连线。
    delete(handle2);
    pause(0.001);
end
tree.child(end + 1) = target;
tree.parent(end + 1) = new_node;
tree.distance(end + 1) = 2 + tree.distance(min_idx);
final_distance = tree.distance(end);
title('RRT, distance:',num2str(final_distance));
current_index = length(tree.child);
while current_index ~= 1
     plot([tree.child(current_index).x,tree.parent(current_index).x],[tree.child(current_index).y,tree.parent(current_index).y],'-','LineWidth',3*resolution,'Color',[0.8500 0.3250 0.0980]);
     for i = 1:length(tree.child)
         if tree.child(i).x == tree.parent(current_index).x
             if tree.child(i).y == tree.parent(current_index).y
                current_index = i;
                break
            end
        end
     end
end

RRTStar主代码

function RRT()
clc
clear all
close all;
[start,target,max_x,max_y,wall,resolution] = creat_map_ob();
figure;%创建新窗口
axis([0 max_x 0 max_y]);
set(gca,'XTick',0:resolution:max_x);%x轴的间隔为1
set(gca,'YTick',0:resolution:max_y);%y轴的间隔为1
grid on;
title('RRT');
xlabel('x');
ylabel('y');
hold on;
for i = 1:1:size(wall,1)
    fill([wall(i,1),wall(i,1) + wall(i,3),wall(i,1) + wall(i,3),wall(i,1)],...
        [wall(i,2),wall(i,2),wall(i,2) + wall(i,4),wall(i,2) + wall(i,4)],'k');
end
plot(start.x,start.y,'b^','MarkerFaceColor','b','MarkerSize',20*resolution);%绘制初始点
text(start.x + 1,start.y + 0.5,'Start');
plot(target.x,target.y,'m^','MarkerFaceColor','m','MarkerSize',20*resolution);%绘制目标点
text(target.x + 1,target.y + 0.5,'Target');
%初始化随机树
tree.child = [];%存储所有节点
tree.parent = [];%存储节点的父节点
tree.distance = [];%存储child到起点的距离
 
tree.child = start;
tree.parent = start;
tree.distance = 0;
radius = 5;
%存储新衍生的节点，起点初始化
%target_distance用于存储new_node到终点的距离
new_node.x = start.x;
new_node.y = start.y;
 
target_distance = sqrt((target.x-new_node.x)^2+(target.y-new_node.y)^2);
grow_distance = 1;
target_radius = 1.5;
 
line1.l = [];
line1.id = [];
while target_distance > target_radius
    random_point.x = max_x*rand();
    random_point.y = max_y*rand();
 
    handle1 = plot(random_point.x,random_point.y,'p','MarkerEdgeColor',[0.9290 0.6940 0.1250],'MarkerFaceColor',[0.9290 0.6940 0.1250],'MarkerSize',10*resolution);
    [angle,min_idx] = find_closet_node(random_point.x,random_point.y,tree);
    %pause(0.01);
    handle2 = plot([tree.child(min_idx).x,random_point.x],[tree.child(min_idx).y,random_point.y],'-','Color',[0.7 0.7 0.7],'LineWidth',0.8*resolution); 
    %根据步长选择新节点
    %pause(0.01);
    new_node.x = tree.child(min_idx).x + grow_distance*cos(angle);
    new_node.y = tree.child(min_idx).y + grow_distance*sin(angle);
    handle3 = plot(new_node.x,new_node.y,'.r','MarkerFaceColor','r','MarkerSize',10*resolution);
    step = 0.005;
    %判断路径是否位于障碍物之中
    %更新
    pause(0.001);
    if isobstacle(wall,step,tree,new_node,min_idx,angle) == 1
        target_distance = sqrt((target.x-new_node.x)^2 + (target.y-new_node.y)^2);
        %删除红色点
        delete(handle3);
        plot(new_node.x,new_node.y,'.g','MarkerFaceColor','g','MarkerSize',10*resolution); % draw the new node
        %画新连接线
        handle4 = plot([tree.child(min_idx).x,new_node.x],[tree.child(min_idx).y,new_node.y],'-k','LineWidth',0.8*resolution);
       % pause(0.001);
        %删除随机点
        delete(handle1);
        %删除最近点与随机点连线。
        delete(handle2);
        %pause(0.001);
        theta = linspace(0, 2*pi, 100);
        handle6 = plot(new_node.x+radius*cos(theta),new_node.y+radius*sin(theta),'b--');
        [minid,min_dis_fs,newid] = chooseparent(wall,tree,radius,min_idx,new_node,step,resolution);
        tree.child(end + 1) = new_node;
        tree.parent(end + 1) = tree.child(minid);
        tree.distance(end + 1) = min_dis_fs;
        delete(handle4);
 
        line1.l(end+1) = plot([tree.child(minid).x,new_node.x],[tree.child(minid).y,new_node.y],'-k','LineWidth',0.8*resolution);
        line1.id(end+1) = length(tree.child);
        [deid] = rewire(wall,tree,minid,new_node,step,newid,min_dis_fs);
        if(deid ~= 0)
           for j = 1:1:length(line1.id)
               if(line1.id(j) == deid)
                   %pause(2);
                 delete (line1.l(j));
                 break
               end
           end
       % pause(2);
       line1.l(j) = plot([tree.child(deid).x,new_node.x],[tree.child(deid).y,new_node.y],'-k','LineWidth',0.8*resolution);
       %pause(2);
        end
        delete(handle6);
    else
        %删除随机点
    %pause(0.001);
    delete(handle3);
    delete(handle1);
    %删除最近点与随机点连线。
    delete(handle2);
    %pause(0.001);
    end
    
end
tree.child(end + 1) = target;
tree.parent(end + 1) = new_node;
tree.distance(end + 1) = 2 + tree.distance(min_idx);
final_distance = tree.distance(end);
title('RRT, distance:',num2str(final_distance));
current_index = length(tree.child);
while current_index ~= 1
     plot([tree.child(current_index).x,tree.parent(current_index).x],[tree.child(current_index).y,tree.parent(current_index).y],'-','LineWidth',3*resolution,'Color',[0.8500 0.3250 0.0980]);
     for i = 1:length(tree.child)
         if tree.child(i).x == tree.parent(current_index).x
             if tree.child(i).y == tree.parent(current_index).y
                current_index = i;
                break
            end
        end
     end
end

rewire代码

function [deid] = rewire(wall,tree,minid,new_node,step,newid,min_dis_fs)
deid = 0;
for i = 1:1:length(newid)
    if(newid(i) ~= minid)
        newcost = min_dis_fs + sqrt((tree.child(newid(i)).x-new_node.x)^2+(tree.child(newid(i)).y-new_node.y)^2);
        if(newcost < tree.distance(newid(i)))
            angle = atan2(new_node.y-tree.child(newid(i)).y,new_node.x-tree.child(newid(i)).x);
            if isobstacle(wall,step,tree,new_node,newid(i),angle) == 1
                tree.parent(newid(i)) = new_node;
                tree.distance(newid(i)) = newcost;
                deid = newid(i);
            end
        end
    end
end

RRT-connect算法精讲

import math
import random

import matplotlib.pyplot as plt
import numpy as np
from celluloid import Camera  # 保存动图时用，pip install celluloid
import operator
import copy


class RRT_Connect:
    """
    Class for RRT_Connect planning
    """

    class Node:
        """
        创建节点
        """

        def __init__(self, x, y):
            self.x = x  # 节点坐标
            self.y = y
            self.path_x = [] # 路径，作为画图的数据，也可以理解成保存的边集
            self.path_y = []
            self.parent = None #父节点

    class AreaBounds:
        """区域大小
        """
        def __init__(self, area):
            self.xmin = float(area[0])
            self.xmax = float(area[1])
            self.ymin = float(area[2])
            self.ymax = float(area[3])

    def __init__(self,
            start,
            goal,
            obstacle_list,
            rand_area,
            expand_dis=3.0,
            goal_sample_rate=5,
            max_iter=1000,
            play_area=None,
            robot_radius=0.0,
            ):
        """
        Setting Parameter

        start:起点 [x,y]
        goal:目标点 [x,y]
        obstacleList:障碍物位置列表 [[x,y,size],...]
        rand_area: 采样区域 x,y ∈ [min,max]
        play_area: 约束随机树的范围 [xmin,xmax,ymin,ymax]
        robot_radius: 机器人半径
        expand_dis: 扩展的步长
        goal_sample_rate: 采样目标点的概率，百分制.default: 5，即表示5%的概率直接采样目标点

        """
        self.start = self.Node(start[0], start[1]) # 根节点
        self.end = self.Node(goal[0], goal[1]) 
        self.min_rand = rand_area[0]
        self.max_rand = rand_area[1]
        if play_area is not None:
            self.play_area = self.AreaBounds(play_area)
        else:
            self.play_area = None
        self.expand_dis = expand_dis
        self.goal_sample_rate = goal_sample  _rate
        self.max_iter = max_iter
        self.obstacle_list = obstacle_list
        self.node_list_1 = []
        self.node_list_2 = []
        self.robot_radius = robot_radius

    def planning(self, animation=True,camara=None):
        """
        rrt path planning

        animation: flag for animation on or off

        camara: 是否保存动图
        """

        # 将起点作为根节点x_{init}​，加入到随机树的节点集合中。
        self.node_list_1 = [self.start]
        self.node_list_2 = [self.end]
        for i in range(self.max_iter):
            # 从可行区域内随机选取一个节点q_{rand}
            rnd_node = self.sample_free()  

            # 已生成的树中利用欧氏距离判断距离q_{rand}​最近的点q_{near}。
            nearest_ind_1 = self.get_nearest_node_index(self.node_list_1, rnd_node)
            nearest_node_1 = self.node_list_1[nearest_ind_1]  
            # 从q_{near}与q_{rand}的连线方向上扩展固定步长u，得到新节点 q_{new}
            new_node_1 = self.steer(nearest_node_1, rnd_node, self.expand_dis)

            
            # 第一棵树，如果在可行区域内，且q_{near}与q_{new}之间无障碍物
            if self.is_inside_play_area(new_node_1, self.play_area) and self.obstacle_free(new_node_1, self.obstacle_list, self.robot_radius):
                self.node_list_1.append(new_node_1)
                # 扩展完第一棵树的新节点$x_{𝑛𝑒𝑤}$后，以这个新的目标点$x_{𝑛𝑒𝑤}$作为第二棵树扩展的方向。
                nearest_ind_2 = self.get_nearest_node_index(self.node_list_2, new_node_1)
                nearest_node_2 = self.node_list_2[nearest_ind_2]  
                new_node_2 = self.steer(nearest_node_2, new_node_1, self.expand_dis)
                # 第二棵树
                if self.is_inside_play_area(new_node_2, self.play_area) and self.obstacle_free(new_node_2, self.obstacle_list, self.robot_radius):
                    self.node_list_2.append(new_node_2)
                    while True:
                        new_node_2_ = self.steer(new_node_2, new_node_1, self.expand_dis)
                        if self.obstacle_free(new_node_2_, self.obstacle_list, self.robot_radius):
                            self.node_list_2.append(new_node_2_)
                            new_node_2 = new_node_2_
                        else:
                            break
                        # print([new_node_2.x,new_node_2.y], [new_node_1.x,new_node_1.y])
                        # 当$𝑞′_{𝑛𝑒𝑤}=𝑞_{𝑛𝑒𝑤}$时，表示与第一棵树相连，算法结束
                        if operator.eq([new_node_2.x,new_node_2.y], [new_node_1.x,new_node_1.y]):
                            return self.generate_final_path()
            
            # 考虑两棵树的平衡性，即两棵树的节点数的多少，交换次序选择“小”的那棵树进行扩展。
            # 不过不交换的情况下好像搜索速度还更快
            if len(self.node_list_1)>len(self.node_list_2):
                list_tmp = copy.deepcopy(self.node_list_1)
                self.node_list_1 = copy.deepcopy(self.node_list_2)
                self.node_list_2 = list_tmp


            if animation and i % 5 ==0:
                self.draw_graph(rnd_node,new_node_1, camara)

        return None  # cannot find path

    def steer(self, from_node, to_node, extend_length=float("inf")):
        """连线方向扩展固定步长查找x_new

        Args:
            from_node (_type_): x_near
            to_node (_type_): x_rand
            extend_length (_type_, optional): 扩展步长u. Defaults to float("inf").

        Returns:
            _type_: _description_
        """
        # 利用反正切计算角度, 然后利用角度和步长计算新坐标
        d, theta = self.calc_distance_and_angle(from_node, to_node)

        # 如果$q_{near}$与$q_{rand}$间的距离小于步长，则直接将$q_{rand}$作为新节点$q_{new}$
        if extend_length >= d:
            new_x = to_node.x
            new_y = to_node.y
        else:
            new_x = from_node.x+math.cos(theta)
            new_y = from_node.y+math.sin(theta)
        new_node_1 = self.Node(new_x,new_y)
        new_node_1.path_x = [from_node.x] # 边集
        new_node_1.path_y = [from_node.y]
        new_node_1.path_x.append(new_x)
        new_node_1.path_y.append(new_y)

        new_node_1.parent = from_node

        return new_node_1



    def generate_final_path(self):
        """生成路径
        Args:
        Returns:
            _type_: _description_
        """
        path_1 = []
        node = self.node_list_1[-1]
        while node.parent is not None:
            path_1.append([node.x, node.y])
            node = node.parent
        path_1.append([node.x, node.y])

        path_2 = []
        node = self.node_list_2[-1]
        while node.parent is not None:
            path_2.append([node.x, node.y])
            node = node.parent
        path_2.append([node.x, node.y])

        path=[]
        for i in range(len(path_1)-1,-1,-1):
            path.append(path_1[i])
        for i in range(len(path_2)):
            path.append(path_2[i])
        
        return path

    def calc_dist(self, x1, y1,x2,y2):
        """计算距离
        """
        dx = x1 - x2
        dy = y1 - y2
        return math.hypot(dx, dy)

    def sample_free(self):
        # 以（100-goal_sample_rate）%的概率随机生长，(goal_sample_rate)%的概率朝向目标点生长
        if random.randint(0, 100) > self.goal_sample_rate:
            rnd = self.Node(
                random.uniform(self.min_rand, self.max_rand),
                random.uniform(self.min_rand, self.max_rand))
        else:  # goal point sampling
            rnd = self.Node(self.end.x, self.end.y)
        return rnd

    def draw_graph(self, rnd=None,rnd_2=None, camera=None):
        if camera==None:
            plt.clf()
        # for stopping simulation with the esc key.
        plt.gcf().canvas.mpl_connect(
            'key_release_event',
            lambda event: [exit(0) if event.key == 'escape' else None])
        # 画随机点
        if rnd is not None:
            plt.plot(rnd.x, rnd.y, "^k")
            if self.robot_radius > 0.0:
                self.plot_circle(rnd.x, rnd.y, self.robot_radius, '-r')
        if rnd_2 is not None:
            plt.plot(rnd_2.x, rnd_2.y, "^r")
            if self.robot_radius > 0.0:
                self.plot_circle(rnd_2.x, rnd_2.y, self.robot_radius, '-b')
        # 画已生成的树
        for node in self.node_list_1:
            if node.parent:
                plt.plot(node.path_x, node.path_y, "-g")
        for node in self.node_list_2:
            if node.parent:
                plt.plot(node.path_x, node.path_y, "-g")
        # 画障碍物
        for (ox, oy, size) in self.obstacle_list:
            self.plot_circle(ox, oy, size)

        # 如果约定了可行区域，则画出可行区域
        if self.play_area is not None:
            plt.plot([self.play_area.xmin, self.play_area.xmax,
                self.play_area.xmax, self.play_area.xmin,
                self.play_area.xmin],
                [self.play_area.ymin, self.play_area.ymin,
                self.play_area.ymax, self.play_area.ymax,
                self.play_area.ymin],
                "-k")

        # 画出起点和目标点
        plt.plot(self.start.x, self.start.y, "xr")
        plt.plot(self.end.x, self.end.y, "xr")
        plt.axis("equal")
        plt.axis([-2, 15, -2, 15])
        plt.grid(True)
        plt.pause(0.01)
        if camera!=None:
            camera.snap()
    # 静态方法无需实例化，也可以实例化后调用，静态方法内部不能调用self.的变量
    @staticmethod
    def plot_circle(x, y, size, color="-b"):  # pragma: no cover
        deg = list(range(0, 360, 5))
        deg.append(0)
        xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
        yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
        plt.plot(xl, yl, color)

    @staticmethod
    def get_nearest_node_index(node_list_1, rnd_node):
        dlist = [(node.x - rnd_node.x)**2 + (node.y - rnd_node.y)**2
                 for node in node_list_1]
        minind = dlist.index(min(dlist))

        return minind

    @staticmethod
    def is_inside_play_area(node, play_area):

        if play_area is None:
            return True  # no play_area was defined, every pos should be ok

        if node.x < play_area.xmin or node.x > play_area.xmax or \
           node.y < play_area.ymin or node.y > play_area.ymax:
            return False  # outside - bad
        else:
            return True  # inside - ok

    @staticmethod
    def obstacle_free(node, obstacleList, robot_radius):

        if node is None:
            return False

        for (ox, oy, size) in obstacleList:
            dx_list = [ox - x for x in node.path_x]
            dy_list = [oy - y for y in node.path_y]
            d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)]

            if min(d_list) <= (size+robot_radius)**2:
                return False  # collision

        return True  # safe

    @staticmethod
    def calc_distance_and_angle(from_node, to_node):
        """计算两个节点间的距离和方位角

        Args:
            from_node (_type_): _description_
            to_node (_type_): _description_

        Returns:
            _type_: _description_
        """
        dx = to_node.x - from_node.x
        dy = to_node.y - from_node.y
        d = math.hypot(dx, dy)
        theta = math.atan2(dy, dx)
        return d, theta


def main(gx=6.0, gy=10.0):
    print("start " + __file__)
    fig = plt.figure(1)

    camera = Camera(fig) # 保存动图时使用
    camera = None # 不保存动图时，camara为None
    show_animation = True
    # ====Search Path with RRT_Connect====
    obstacleList = [(3, 6, 1), (3, 8, 2), (3, 10, 2), (7, 5, 1),
            (9, 5, 2), (8, 10, 1), (7, 5, 2), (10, 5, 2)]  # [x, y, radius]
    # Set Initial parameters
    rrt = RRT_Connect(
        start=[0, 0],
        goal=[gx, gy],
        rand_area=[-2, 15],
        obstacle_list=obstacleList,
        play_area=[0, 10, 0, 14],
        robot_radius=0.8
    )
    path = rrt.planning(animation=show_animation,camara=camera)
    if path is None:
        print("Cannot find path")
    else:
        path = np.array(path)
        print(path)
        print("found path!!")

        # Draw final path
        if show_animation:
            rrt.draw_graph(camera=camera)
            plt.grid(True)
            plt.pause(0.01)  
            plt.plot(path[:,0], path[:,1], '-r')
            if camera!=None:
                camera.snap()
                animation = camera.animate()
                animation.save('trajectory.gif')
            plt.figure(2)
            plt.axis("equal")
            plt.axis([-2, 15, -2, 15])
            plt.grid(True)
            plt.plot(path[:,0], path[:,1], '-r')
            plt.show()




if __name__ == '__main__':
    main()

OMPL简介

1. 使用SimplSetup类我们就需要实现：StateValidityChecker与StateSpace
2. 不适用SimpleSetup类我们就需要实现：StateValidityChecker、SpaceInformation、ProblemDefinition、planner、StateSpace
3. 我看了后面教程才知道，使用OMPL的过程中还要按照要求去实现一些函数，因此下面的代码看起来可能有些难以理解，不过这是官方第一个教程，重点是理解上面图中的关系。

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/SimpleSetup.h>

#include <ompl/config.h>
#include <iostream>

namespace ob = ompl::base;
namespace og = ompl::geometric;

bool isStateValid(const ob::State *state)
{
    // cast the abstract state type to the type we expect
    // 这里将抽象基类装换成复合状态空间SE(3),这是看了后面教程才知道的
    const auto *se3state = state->as<ob::SE3StateSpace::StateType>();

    // extract the first component of the state and cast it to what we expect
    // 从复合状态空间SE(3)中提取R^3
    const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);

    // extract the second component of the state and cast it to what we expect
    // 从复合状态空间SE(3)中提取SO(3)
    const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);

    // check validity of state defined by pos & rot


    // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
    return (const void*)rot != (const void*)pos;
}

void plan()
{
    // construct the state space we are planning in
    // 创建规划所在的状态空间
    auto space(std::make_shared<ob::SE3StateSpace>());

    // set the bounds for the R^3 part of SE(3)
    // 创建Rn的状态空间的边界
    ob::RealVectorBounds bounds(3);
    bounds.setLow(-1);
    bounds.setHigh(1);
    // 设置状态空间的边界
    space->setBounds(bounds);

    // construct an instance of  space information from this state space
    // 创建一个SpaceInformation类
    auto si(std::make_shared<ob::SpaceInformation>(space));

    // set state validity checking for this space
    // SpaceInformation对象进行状态点有效性检测，这里直接使用isStateValid，感觉是一样啊
    si->setStateValidityChecker(isStateValid);

    // create a random start state
    ob::ScopedState<> start(space);
    start.random();

    // create a random goal state
    ob::ScopedState<> goal(space);
    goal.random();

    // create a problem instance
    // 构建问题定义对象，需要SpaceInformation对象
    auto pdef(std::make_shared<ob::ProblemDefinition>(si));

    // set the start and goal states
    // 为问题定义对象设置起始点与终点
    pdef->setStartAndGoalStates(start, goal);

    // create a planner for the defined space
    // 创建规划器对象，需要SpaceInformation对象
    auto planner(std::make_shared<og::RRTConnect>(si));

    // set the problem we are trying to solve for the planner
    // 设置规划器解决的问题对象，需要ProblemDefinition
    planner->setProblemDefinition(pdef);

    // perform setup steps for the planner
    // 启动设置？
    planner->setup();

    // print the settings for this space
    si->printSettings(std::cout);

    // print the problem settings
    pdef->print(std::cout);

    // attempt to solve the problem within one second of planning time
    ob::PlannerStatus solved = planner->ob::Planner::solve(1.0);

    if (solved)
    {
        // get the goal representation from the problem definition (not the same as the goal state)
        // and inquire about the found path
        // 从问题对象中获取结果
        ob::PathPtr path = pdef->getSolutionPath();
        std::cout << "Found solution:" << std::endl;

        // print the path to screen
        path->print(std::cout);
    }
    else
        std::cout << "No solution found" << std::endl;
}

void planWithSimpleSetup()
{
    // construct the state space we are planning in
    // 创建状态空间对象
    auto space(std::make_shared<ob::SE3StateSpace>());

    // set the bounds for the R^3 part of SE(3)
    // 创建状态空间组件的边界，不理解，每个维度最小值为-1，最大1么
    ob::RealVectorBounds bounds(3);
    bounds.setLow(-1);
    bounds.setHigh(1);
    // 设置状态空间组件的边界
    space->setBounds(bounds);

    // define a simple setup class
    // 创建一个SimpleSetup对象，该对象自动实例化SpaceInformation,ProblemDefinition
    og::SimpleSetup ss(space);

    // set state validity checking for this space
    // 设置状态有效性检查类，本来是要传入一个类，此处传入的是一个函数，StateValidityCheckerFn中有说明
    ss.setStateValidityChecker([](const ob::State *state) { return isStateValid(state); });

    // create a random start state
    // 创建ScopedState(域内状态对象么。。。)
    ob::ScopedState<> start(space);
    // 设置这个状态为随机值
    start.random();

    // create a random goal state
    // 同上
    ob::ScopedState<> goal(space);
    goal.random();
    
    // set the start and goal states
    // 设置起始点与终点
    ss.setStartAndGoalStates(start, goal);

    // this call is optional, but we put it in to get more output information
    ss.setup();
    ss.print();

    // attempt to solve the problem within one second of planning time
    // 设置完毕开始求解
    ob::PlannerStatus solved = ss.solve(1.0);

    if (solved)
    {
        std::cout << "Found solution:" << std::endl;
        // print the path to screen
        // 简化求解的结果？
        ss.simplifySolution();
        // 输出结果
        ss.getSolutionPath().print(std::cout);
    }
    else
        std::cout << "No solution found" << std::endl;
}

int main(int /*argc*/, char ** /*argv*/)
{
    std::cout << "OMPL version: " << OMPL_VERSION << std::endl;

    plan();

    std::cout << std::endl << std::endl;

    planWithSimpleSetup();

    return 0;
}

上述代码流程的简单总结：
一、使用SimpleSetup

• 创建SE3StateSpace对象
• 创建RealVectorBounds
• 设置SE3StateSpace的边界
• 创建SimpleSetup对象，需要SE3StateSpace
• 调用SimpleSetup的setStateValidityChecker，状态的有效性检测
• 创建起始点与终点，ScopedState
• 调用SimpleSetup的setStartAndGoalStates，设置起点终点
• 调用SimpleSetup的solve，解算
• 调用SimpleSetup的simplifySolution，简化结果？
• 调用SimpleSetup的getSolutionPath，获取路径PathGeometric

二、不使用SimpleSetup

• 创建SE3StateSpace对象
• 创建RealVectorBounds，并设置边界
• 设置SE3StateSpace的边界
• 创建SpaceInformation对象，需要SE3StateSpace
• 调用SpaceInformation的setStateValidityChecker，状态的有效性检测
• 创建起始点与终点，ScopedState
• 创建ProblemDefinition，需要SpaceInformation
• 调用ProblemDefinition的setStartAndGoalStates，设置起点终点
• 创建planner，需要SpaceInformation
• 调用planner的setProblemDefinition，需要ProblemDefinition
• 调用planner的solve，求解
• 调用planner的getSolutionPath，获得路径