new rtt* algo
This commit is contained in:
dave
parent
c982c60592
commit
37c3cf151b
|
|
@ -729,7 +729,7 @@ function init() {
|
|||
#v_tag = stigmergy.create(m_lockstig)
|
||||
#uav_initstig()
|
||||
# go to diff. height since no collision avoidance implemented yet
|
||||
TARGET_ALTITUDE = 3.0 + id * 1.25
|
||||
#TARGET_ALTITUDE = 3.0 + id * 1.25
|
||||
statef=turnedoff
|
||||
UAVSTATE = "TURNEDOFF"
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,233 @@
|
|||
#####
|
||||
# RRT* Path Planing
|
||||
# START, GOAL, TOL vec2
|
||||
# map table-based matrix
|
||||
#####
|
||||
|
||||
map = {.nb_col=100, .nb_row=100, .mat={}}
|
||||
|
||||
function RRTSTAR(START,GOAL,TOL) {
|
||||
# RRT_STAR
|
||||
HEIGHT = map.nb_col
|
||||
WIDTH = map.nb_row
|
||||
RADIUS = 2
|
||||
|
||||
rng.setseed(11)
|
||||
|
||||
goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y}
|
||||
arrayOfPoints = {.nb_col=2, .nb_row=1, .mat={.0=START.x,.1=START.y}}
|
||||
numberOfPoints = 1
|
||||
Q = {.nb_col=5,.nb_row=1,.mat={.0=START.x,.1=START.y,.2=0,.3=1,.4=0}}
|
||||
log("Created ",Q)
|
||||
|
||||
# Open space or obstacle
|
||||
# DISPLAY_patchwork(map);
|
||||
|
||||
goalReached = 0;
|
||||
while(goalReached == 0) {
|
||||
# Point generation
|
||||
x = -HEIGHT*rng.uniform(1.0)-1;
|
||||
y = WIDTH*rng.uniform(1.0)+1;
|
||||
log("First trial point (", rng.uniform(1.0), "): ", x, " ", y)
|
||||
|
||||
pointList = findPointsInRadius(x,y,Q,RADIUS);
|
||||
|
||||
log(pointList.nb_col,pointList.nb_row,pointList.mat)
|
||||
|
||||
# Find connection that provides the least cost to come
|
||||
nbCount = 0;
|
||||
minCounted = inf;
|
||||
minCounter = 0;
|
||||
|
||||
if(pointList.nb_col!=0) {
|
||||
i=0
|
||||
while(i<pointList.nb_row){
|
||||
pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}}
|
||||
mat_copyrow(pointNumber,0,pointList,i)
|
||||
|
||||
# Follow the line to see if it intersects anything
|
||||
intersects = doesItIntersect(x,y,math.vec2.new(pointNumber.mat[0],pointNumber.mat[1]),map);
|
||||
|
||||
# If there is no intersection we need consider its connection
|
||||
nbCount = nbCount + 1;
|
||||
if(intersects != 1) {
|
||||
log(pointNumber, "do not intersect (",pointNumber.mat[3],")")
|
||||
distance = math.vec2.length(pointNumber.mat[0]-x,pointNumber.mat[1]-y)+rmap(Q,pointNumber.mat[3],5)
|
||||
|
||||
if(distance < minCounted) {
|
||||
minCounted = distance;
|
||||
minCounter = nbCount;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(minCounter > 0) {
|
||||
arrayOfPoints.nb_row=arrayOfPoints.nb_row+1
|
||||
arrayOfPoints.mat[arrayOfPoints.nb_row]=x
|
||||
arrayOfPoints.mat[arrayOfPoints.nb_row+1]=y
|
||||
numberOfPoints = numberOfPoints + 1;
|
||||
|
||||
wmat(Q,numberOfPoints,0, x)
|
||||
wmat(Q,numberOfPoints,1, y)
|
||||
wmat(Q,numberOfPoints,2, rmat(pointList,minCounter,4));
|
||||
wmat(Q,numberOfPoints,3, numberOfPoints)
|
||||
wmat(Q,numberOfPoints,4, minCounted)
|
||||
|
||||
# Now check to see if any of the other points can be redirected
|
||||
nbCount = 0;
|
||||
i = 0
|
||||
while(i<pointList.nb_col) {
|
||||
pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}}
|
||||
mat_copyrow(pointNumber,0,pointList,i)
|
||||
|
||||
# Follow the line to see if it intersects anything
|
||||
intersects = doesItIntersect(x,y,math.vec2.new(pointNumber.mat[0],pointNumber.mat[1]),map);
|
||||
|
||||
# If there is no intersection we need consider its connection
|
||||
nbCount = nbCount + 1;
|
||||
if(intersects != 1) {
|
||||
# If the alternative path is shorter than change it
|
||||
tmpdistance = math.vec2.length(pointNumber.mat[0]-x,pointNumber.mat[1]-y)+rmap(Q,numberOfPoints,5)
|
||||
if(tmpdistance < rmap(Q,pointNumber.mat[3],5)) {
|
||||
wmat(Q,pointNumber.mat[3],3, numberOfPoints)
|
||||
wmat(Q,pointNumber.mat[3],5, rmap(Q,numberOfPoints,5)+math.vec2.length(pointNumber.mat[0]-x,pointNumber.mat[1]-y))
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
# Check to see if this new point is within the goal
|
||||
if(x < goalBoundary.xmax and x > goalBoundary.xmin and y > goalBoundary.ymin and y < goalBoundary.ymax)
|
||||
goalReached = 1;
|
||||
}
|
||||
} else {
|
||||
# Associate with the closest point
|
||||
pointNum = findClosestPoint(x,y,arrayOfPoints);
|
||||
|
||||
# Follow the line to see if it intersects anything
|
||||
intersects = doesItIntersect(x,y,math.vec2.new(rmat(arrayOfPoints,1,pointNum),rmat(arrayOfPoints,2,pointNum)),map);
|
||||
|
||||
# If there is no intersection we need to add to the tree
|
||||
if(intersects != 1) {
|
||||
arrayOfPoints.nb_row=arrayOfPoints.nb_row+1
|
||||
arrayOfPoints.mat[arrayOfPoints.nb_row]=x
|
||||
arrayOfPoints.mat[arrayOfPoints.nb_row+1]=y
|
||||
numberOfPoints = numberOfPoints + 1;
|
||||
|
||||
wmat(Q,numberOfPoints,0, x)
|
||||
wmat(Q,numberOfPoints,1, y)
|
||||
wmat(Q,numberOfPoints,2, pointNum);
|
||||
wmat(Q,numberOfPoints,3, numberOfPoints)
|
||||
wmat(Q,numberOfPoints,4, rmat(Q,pointNum,5)+math.vec2.length(rmat(Q,pointNum,1)-x,rmat(Q,pointNum,2)-y))
|
||||
|
||||
# Check to see if this new point is within the goal
|
||||
if(x < goalBoundary.xmax and x > goalBoundary.xmin and y > goalBoundary.ymin and y < goalBoundary.ymax)
|
||||
goalReached = 1;
|
||||
}
|
||||
}
|
||||
if(numberOfPoints % 100 == 0) {
|
||||
log(numberOfPoints, " points processed. Still looking for goal.");
|
||||
}
|
||||
}
|
||||
log("Goal found!");
|
||||
}
|
||||
|
||||
function findClosestPoint(x,y,arrayOfPoints) {
|
||||
# Go through each points and find the distances between them and the
|
||||
# target point
|
||||
distance = inf;
|
||||
i=1
|
||||
while(i<arrayOfPoints.nb_row) {
|
||||
range = (x-rmat(arrayOfPoints,1,i))^2+(y-rmat(arrayOfPoints,2,i))^2;
|
||||
|
||||
if(range < distance) {
|
||||
distance = range;
|
||||
pointNumber = i;
|
||||
}
|
||||
i = i + 1
|
||||
}
|
||||
return pointNumber
|
||||
}
|
||||
|
||||
function findPointsInRadius(x,y,arrayOfPoints,RADIUS) {
|
||||
counted = 0;
|
||||
pointList = {.nb_col=arrayOfPoints.nb_col, .nb_row=counted, .mat={}}
|
||||
i=1
|
||||
while(i < arrayOfPoints.nb_row){
|
||||
distance = math.vec2.length(rmat(arrayOfPoints,i,1)-x,rmat(arrayOfPoints,i,2)-y);
|
||||
|
||||
if(distance < RADIUS){
|
||||
counted = counted+1;
|
||||
pointList.nb_row=counted
|
||||
mat_copyrow(pointList,counted,arrayOfPoints,i)
|
||||
}
|
||||
i = i + 1
|
||||
}
|
||||
return pointList
|
||||
}
|
||||
|
||||
function doesItIntersect(x,y,vector) {
|
||||
x = -x;
|
||||
vector.x = -vector.x;
|
||||
distance = math.vec2.length(vector);
|
||||
vec = math.vec2.scale(math.vec2.sub(math.vec2.new(x,y),vector),1/distance);
|
||||
log("doesItIntersect: ",vec)
|
||||
#range = linspace(0,distance,distance*100);
|
||||
i = 1;
|
||||
#plot(y,-x,'b*');
|
||||
while(i<100) {
|
||||
if(block == 0) {
|
||||
range = distance/100.0*i
|
||||
position = math.vec2.sub(math.vec2.new(x,y),math.vec2.scale(vec,range));
|
||||
|
||||
# Find what block we're in right now
|
||||
xi = math.floor(position.x)
|
||||
yi = math.floor(position.y)
|
||||
|
||||
if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) {
|
||||
if(rmap(xi,yi) == 0)
|
||||
return 1;
|
||||
} else
|
||||
return 1;
|
||||
}
|
||||
i = i + 1
|
||||
}
|
||||
return 0
|
||||
}
|
||||
|
||||
# Write to matrix
|
||||
function wmat(mat, row, col, val) {
|
||||
var index = (row-1)*mat.nb_col + col
|
||||
mat.mat[index] = val
|
||||
}
|
||||
|
||||
# Read from matrix
|
||||
function rmat(mat, row, col) {
|
||||
var index = (row-1)*mat.nb_col + col
|
||||
if (mat.mat[index] == nil) {
|
||||
return -1
|
||||
} else {
|
||||
return mat.mat[index]
|
||||
}
|
||||
}
|
||||
|
||||
# copy a full matrix row
|
||||
function mat_copyrow(out,ro,in,ri){
|
||||
var indexI = (ri-1)*in.nb_col
|
||||
var indexO = (ro-1)*out.nb_col
|
||||
i=0
|
||||
while(i<in.nb_col){
|
||||
out.mat[indexO+i]=in.mat[indexI+i]
|
||||
i = i + 1
|
||||
}
|
||||
}
|
||||
|
||||
function make_test_map(){
|
||||
# creates a 5x5 map
|
||||
map = {.nb_col=5, .nb_row=5, .mat={}}
|
||||
index = 0
|
||||
while(index<25){
|
||||
map.mat[index]=0
|
||||
index = index + 1
|
||||
}
|
||||
# puts an obstacle right in the middle
|
||||
map.mat[5*2+3]=1
|
||||
}
|
||||
|
|
@ -3,6 +3,7 @@ include "update.bzz"
|
|||
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
|
||||
include "uavstates.bzz" # require an 'action' function to be defined here.
|
||||
include "vstigenv.bzz"
|
||||
include "rttstar.bzz"
|
||||
|
||||
function action() {
|
||||
statef=action
|
||||
|
|
@ -15,6 +16,8 @@ function init() {
|
|||
statef=turnedoff
|
||||
UAVSTATE = "TURNEDOFF"
|
||||
uav_initstig()
|
||||
make_test_map()
|
||||
RRTSTAR(math.vec2.new(0,0),math.vec2.new(5,5),math.vec2.new(1,1))
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
|
|
|
|||
|
|
@ -43,9 +43,9 @@ namespace buzzuav_closures{
|
|||
|
||||
int buzzros_print(buzzvm_t vm) {
|
||||
int i;
|
||||
char buffer [50] = "";
|
||||
char buffer [100] = "";
|
||||
sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid());
|
||||
for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
|
||||
for(uint32_t i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
|
||||
buzzvm_lload(vm, i);
|
||||
buzzobj_t o = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_pop(vm);
|
||||
|
|
|
|||
Loading…
Reference in New Issue