路径规划基本概念
机械臂视觉操作流程 = 手眼标定 + 目标识别与定位 + 操作姿态分析 + 运动规划 = 手眼标定 + 深度学习 + 运动规划
手眼标定
作用: 获得机器人坐标系和相机坐标系的关系 效果: 目前可达到误差在土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简介
- 使用SimplSetup类我们就需要实现：StateValidityChecker与StateSpace
- 不适用SimpleSetup类我们就需要实现：StateValidityChecker、SpaceInformation、ProblemDefinition、planner、StateSpace
- 我看了后面教程才知道，使用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，获得路径