debug new matrix form for RRT and add blob check for intersection

This commit is contained in:
dave 2017-12-19 18:36:10 -05:00
parent c03d6e269b
commit 94ad933452
5 changed files with 167 additions and 109 deletions

View File

@ -5,8 +5,8 @@ robot_marker = "X"
# copy a full matrix row
function mat_copyrow(out,ro,in,ri){
out[ro] = {}
var icr = 0
while(icr < size(in[ri])) {
var icr = 1
while(icr <= size(in[ri])) {
out[ro][icr]=in[ri][icr]
icr = icr + 1
}
@ -18,11 +18,11 @@ function getvec(t,row){
function init_test_map(len){
map = {}
var indexR = 0
while(indexR < len) {
var indexR = 1
while(indexR <= len) {
map[indexR] = {}
var indexC = 0
while(indexC < len) {
var indexC = 1
while(indexC <= len) {
map[indexR][indexC] = 1.0
indexC = indexC + 1
}
@ -33,22 +33,22 @@ function init_test_map(len){
map[6][5] = 0.0
map[4][5] = 0.0
log("Occupancy grid initialized (",len,"x",len,") with obstacles.")
log("Occupancy grid initialized (",size(map),"x",size(map[1]),") with obstacles.")
}
function init_map(len){
map = {}
var indexR = 0
while(indexR < len) {
var indexR = 1
while(indexR <= len) {
map[indexR] = {}
var indexC = 0
while(indexC < len) {
var indexC = 1
while(indexC <= len) {
map[indexR][indexC] = 1.0
indexC = indexC + 1
}
indexR = indexR + 1
}
log("Occupancy grid initialized (",len,"x",len,").")
log("Occupancy grid initialized (",size(map),"x",size(map[1]),").")
}
function add_obstacle(pos, off, inc_trust) {
@ -57,7 +57,7 @@ function add_obstacle(pos, off, inc_trust) {
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
#log("Add obstacle in cell: ", xi, " ", yi)
old=map[xi][yi]
var old=map[xi][yi]
if(old-inc_trust > 0.0)
map[xi][yi] = old-inc_trust
else
@ -71,7 +71,7 @@ function remove_obstacle(pos, off, dec_trust) {
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0){
#log("Remove obstacle in cell: ", xi, " ", yi)
old=map[xi][yi]
var old=map[xi][yi]
if(old + dec_trust < 1.0) #x,y
map[xi][yi] = old+dec_trust
else
@ -79,6 +79,30 @@ function remove_obstacle(pos, off, dec_trust) {
}
}
function get_occupied_cells(cur_cell){
var i = 1
var occupied_cells = {}
occupied_cells[0] = cur_cell
occupied_cells[1] = math.vec2.new(cur_cell.x + 1, cur_cell.y)
occupied_cells[2] = math.vec2.new(cur_cell.x - 1, cur_cell.y)
occupied_cells[3] = math.vec2.new(cur_cell.x, cur_cell.y + 1)
occupied_cells[4] = math.vec2.new(cur_cell.x, cur_cell.y - 1)
return occupied_cells
}
function is_in(dictionary, x, y){
var i = 0
while(i < size(dictionary)){
if(dictionary[i].x == x and dictionary[i].y == y){
return 1
}
i = i + 1
}
return 0
}
function table_print(t) {
foreach(t, function(key, value) {
log(key, " -> ", value)

View File

@ -14,12 +14,12 @@ function UAVinit_map(m_navigation) {
# create a map big enough for the goal
init_map(2*math.round(math.vec2.length(m_navigation))+10)
# center the robot on the grid
cur_cell = math.vec2.new(math.round(size(map[1])/2.0),math.round(size(map)/2.0))
cur_cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0))
}
function UAVpathPlanner(m_navigation, m_pos) {
# place the robot on the grid
update_curcell(m_pos,0)
cur_cell = getcell(m_pos,0)
# create the goal in the map grid
mapgoal = math.vec2.add(m_navigation,cur_cell)
mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y))
@ -34,7 +34,7 @@ function Kh4pathPlanner(m_goal, m_pos) {
m_goal = math.vec2.new(math.round(m_goal.x*CM2KH4.x), math.round(m_goal.y*CM2KH4.y))
# place the robot on the grid
update_curcell(m_pos,1)
cur_cell = getcell(m_pos,1)
log("Starting from cell: ", cur_cell.x, " ", cur_cell.y)
log("Going to cell: ", m_goal.x, " ", m_goal.y)
@ -44,26 +44,35 @@ function Kh4pathPlanner(m_goal, m_pos) {
return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y))
}
function update_curcell(m_curpos, kh4) {
function getcell(m_curpos, kh4) {
var cell = {}
if(kh4) { # for khepera playground
cur_cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
cur_cell = math.vec2.new(math.round(cur_cell.x*CM2KH4.x), math.round(cur_cell.y*CM2KH4.y))
cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
cell = math.vec2.new(math.round(cell.x*CM2KH4.x), math.round(cell.y*CM2KH4.y))
} else { # for uav map relative to home
cur_cell = math.vec2.add(cur_cell, m_curpos)
cur_cell = math.vec2.new(math.round(m_curpos.x), math.round(m_curpos.y))
cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0))
cell = math.vec2.add(cell, m_curpos)
cell = math.vec2.new(math.round(cell.x), math.round(cell.y))
}
if(cur_cell.x > size(map))
cur_cell.x = size(map)
if(cur_cell.y > size(map[1]))
cur_cell.y = size(map[1])
if(cur_cell.x < 1)
cur_cell.x = 1
if(cur_cell.y < 1)
cur_cell.y = 1
if(cell.x > size(map))
cell.x = size(map)
if(cell.y > size(map[1]))
cell.y = size(map[1])
if(cell.x < 1)
cell.x = 1
if(cell.y < 1)
cell.y = 1
return cell
}
function updateMap(m_pos) {
#getproxobs(m_pos)
UAVgetneiobs (m_pos)
}
function UAVgetneiobs (m_curpos) {
update_curcell(m_curpos,0)
cur_cell = getcell(m_curpos,0)
# add all neighbors as obstacles in the grid
neighbors.foreach(function(rid, data) {
add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0)
@ -72,7 +81,7 @@ function UAVgetneiobs (m_curpos) {
function getneiobs (m_curpos) {
var foundobstacle = 0
update_curcell(m_curpos,1)
cur_cell = getcell(m_curpos,1)
var old_nei = table_copy(nei_cell)
#log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3])
@ -118,7 +127,7 @@ function getneiobs (m_curpos) {
function getproxobs (m_curpos) {
foundobstacle = 0
update_curcell(m_curpos,1)
cur_cell = getcell(m_curpos,1)
reduce(proximity,
function(key, value, acc) {
@ -159,19 +168,21 @@ function getproxobs (m_curpos) {
}
function check_path(m_path, goal_l, m_curpos, kh4) {
pathisblocked = 0
nb=goal_l
update_curcell(m_curpos,kh4)
Cvec = cur_cell
while(nb < size(m_path) and nb <= goal_l+1) {
Nvec = getvec(m_path, nb)
if(kh4==0)
Nvec=vec_from_gps(Nvec.x,Nvec.y)
if(doesItIntersect(Cvec, Nvec)){
log("Obstacle in the way ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")")
var pathisblocked = 0
var nb = goal_l
cur_cell = getcell(m_curpos,kh4)
var Cvec = cur_cell
while(nb <= size(m_path) and nb <= goal_l+1) {
var Nvec = getvec(m_path, nb)
if(kh4 == 0) {
Nvec = vec_from_gps(Nvec.x,Nvec.y,1)
Nvec = getcell(Nvec,kh4)
}
if(doesItIntersect(Cvec, Nvec)) {
log("Path blocked ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")")
pathisblocked = 1
}
Cvec=Nvec
Cvec = Nvec
nb = nb + 1
}
@ -184,12 +195,16 @@ function RRTSTAR(GOAL,TOL) {
RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive
goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y}
var goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y}
#table_print(goalBoundary)
numberOfPoints = 1
arrayOfPoints = {}
var numberOfPoints = 1
var arrayOfPoints = {}
arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y}
Path = {}
mat_copyrow(Path,1,arrayOfPoints,1)
# RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance
Q = {}
Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0}
goalReached = 0;
timeout = 0
@ -202,7 +217,7 @@ function RRTSTAR(GOAL,TOL) {
pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1))
#log("Point generated: ", pt.x, " ", pt.y)
pointList = findPointsInRadius(pt,Q,RADIUS);
var pointList = findPointsInRadius(pt,Q,RADIUS);
# Find connection that provides the least cost to come
nbCount = 0;
@ -210,7 +225,7 @@ function RRTSTAR(GOAL,TOL) {
minCounter = 0;
if(size(pointList) != 0) {
#log("Found ", size(pointList), " close point:", pointList.mat)
log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3])
ipt=1
while(ipt <= size(pointList)) {
pointNumber = {}
@ -224,7 +239,7 @@ function RRTSTAR(GOAL,TOL) {
nbCount = nbCount + 1;
if(intersects != 1) {
#log(pointNumber, "do not intersect (",pointNumber.mat[3],")")
distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][3]][5]
var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][3]][5]
if(distance < minCounted) {
minCounted = distance;
@ -235,9 +250,11 @@ function RRTSTAR(GOAL,TOL) {
}
if(minCounter > 0) {
numberOfPoints = numberOfPoints + 1;
arrayOfPoints[numberOfPoints] = {}
arrayOfPoints[numberOfPoints][1]=pt.x
arrayOfPoints[numberOfPoints][2]=pt.y
Q[numberOfPoints] = {}
Q[numberOfPoints][1] = pt.x
Q[numberOfPoints][2] = pt.y
Q[numberOfPoints][3] = pointList[minCounter][4]
@ -249,9 +266,10 @@ function RRTSTAR(GOAL,TOL) {
# Now check to see if any of the other points can be redirected
nbCount = 0;
ipt = 1
while(ipt < size(pointList)) {
while(ipt <= size(pointList)) {
pointNumber = {}
mat_copyrow(pointNumber,1,pointList,ipt)
#log("pN ", size(pointNumber), ", ipt: ", ipt, "prec: ", pointNumber[1][4])
# Follow the line to see if it intersects anything
intersects = doesItIntersect(pt,getvec(pointNumber,1));
@ -262,6 +280,7 @@ function RRTSTAR(GOAL,TOL) {
if(intersects != 1) {
# If the alternative path is shorter than change it
tmpdistance = Q[numberOfPoints][5]+math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))
#log("Q: ", size(Q), "x", size(Q[1]))
if(tmpdistance < Q[pointNumber[1][4]][5]) {
Q[pointNumber[1][4]][3] = numberOfPoints
Q[pointNumber[1][4]][5] = tmpdistance
@ -285,9 +304,11 @@ function RRTSTAR(GOAL,TOL) {
# If there is no intersection we need to add to the tree
if(intersects != 1) {
numberOfPoints = numberOfPoints + 1;
arrayOfPoints[numberOfPoints] = {}
arrayOfPoints[numberOfPoints][1]=pt.x
arrayOfPoints[numberOfPoints][2]=pt.y
Q[numberOfPoints] = {}
Q[numberOfPoints][1] = pt.x
Q[numberOfPoints][2] = pt.y
Q[numberOfPoints][3] = pointNum
@ -320,51 +341,51 @@ function RRTSTAR(GOAL,TOL) {
function findClosestPoint(point,aPt) {
# Go through each points and find the distances between them and the
# target point
distance = 999
pointNumber = -1
ifcp=1
var distance = 999
var pointNb = -1
var ifcp=1
while(ifcp <= size(aPt)) {
range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp)))
var range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp)))
if(range < distance) {
distance = range;
pointNumber = ifcp;
pointNb = ifcp;
}
ifcp = ifcp + 1
}
return pointNumber
return pointNb
}
# Find the closest point in the tree
function findPointsInRadius(point,q,r) {
pointList = {}
var ptList = {}
var counted = 0;
var iir = 1
while(iir <= size(q)) {
tmpvv = getvec(q,iir)
var tmpvv = getvec(q,iir)
#log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y)
distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point))
var distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point))
if(distance < r) {
counted = counted+1;
mat_copyrow(pointList,counted,q,iir)
mat_copyrow(ptList,counted,q,iir)
}
iir = iir + 1
}
return pointList
return ptList
}
# check if the line between 2 point intersect an obstacle
function doesItIntersect(point,vector) {
#log("DiI :", point.x, " ", point.y," ", vector.x," ", vector.y)
dif = math.vec2.sub(point,vector)
distance = math.vec2.length(dif)
if(distance==0.0){
var dif = math.vec2.sub(point,vector)
var distance = math.vec2.length(dif)
if(distance == 0.0){
# Find what block we're in right now
var xi = math.round(point.x) #+1
var yi = math.round(point.y) #+1
if(xi!=cur_cell.x and yi!=cur_cell.y){
if(xi >= size(map) and yi >= size(map[1]) and xi != cur_cell.x and yi != cur_cell.y) {
if(map[xi][yi] > 0.5)
return 1
else
@ -372,21 +393,22 @@ function doesItIntersect(point,vector) {
} else
return 0
}
vec = math.vec2.scale(dif,1.0/distance)
pathstep = size(map[1]) - 2
var vec = math.vec2.scale(dif,1.0/distance)
var pathstep = size(map) - 2
idii = 1.0
while(idii <= pathstep) {
range = distance*idii/pathstep
pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range));
var range = distance*idii/pathstep
var pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range));
# Find what block we're in right now
var xi = math.round(pos_chk.x) #+1
var yi = math.round(pos_chk.y) #+1
#log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range)
#log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range, "(Map size: ", size(map), ")")
if(xi!=cur_cell.x and yi!=cur_cell.y){
if(xi <= size(map[1]) and yi <= size(map) and xi > 0 and yi > 0) {
if(is_in(get_occupied_cells(cur_cell), xi, yi) == 0){
#if(xi!=cur_cell.x and yi!=cur_cell.y) {
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
if(map[xi][yi] < 0.5) { # because obstacle use trust values
#log("Obstacle in the way(", xi, " ", yi, ": ", map[xi][yi], ")!")
return 1;
@ -403,20 +425,28 @@ function doesItIntersect(point,vector) {
}
function getPathGPS(Q,nb){
#log("-----------CONVERTING PATH!!!")
path={}
var npt=0
var isrecursive = 0
# get the path from the point list
while(nb!=1){
while(nb > 1 and isrecursive!=1){
npt=npt+1
path[npt] = {}
path[npt][1]=Q[nb][1]
path[npt][2]=Q[nb][2]
if(nb != Q[Q[nb][3]][3])
nb = Q[nb][3];
else {
isrecursive = 1
path={}
log("ERROR - Recursive path !!!")
}
}
# re-order the list and make the path points absolute
pathR={}
var totpt = npt
var totpt = npt + 1
while(npt > 0){
tmpgoal = gps_from_vec(math.vec2.sub(getvec(path,npt),cur_cell))
pathR[totpt-npt] = {}
@ -433,7 +463,7 @@ function getPath(Q,nb){
var npt=0
# log("get the path from the point list")
while(nb!=1){
while(nb > 1){
npt=npt+1
path[npt] = {}
path[npt][1]=Q[nb][1]
@ -444,7 +474,7 @@ function getPath(Q,nb){
# log("re-order the list")
# table_print(path.mat)
pathR={}
var totpt = npt
var totpt = npt + 1
while(npt > 0){
tmpgoal = getvec(path,npt)
pathR[totpt-npt] = {}

View File

@ -3,6 +3,7 @@
# FLIGHT-RELATED FUNCTIONS
#
########################################
include "vec2.bzz"
include "rrtstar.bzz"
TARGET_ALTITUDE = 15.0 # m.
@ -14,7 +15,6 @@ GOTODIST_TOL = 0.5 # m.
GOTOANG_TOL = 0.1 # rad.
cur_goal_l = 0
rc_State = 0
homegps = {.lat=0, .long=0}
function uav_initswarm() {
s = swarm.create(1)
@ -34,6 +34,7 @@ function idle() {
function takeoff() {
UAVSTATE = "TAKEOFF"
statef=takeoff
homegps = {.lat=position.latitude, .long=position.longitude}
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, action, land, -1)
@ -61,7 +62,7 @@ function land() {
function set_goto(transf) {
UAVSTATE = "GOTOGPS"
statef=function() {
gotoWP(transf)
gotoWPRRT(transf)
}
if(rc_goto.id==id){
@ -93,10 +94,8 @@ function picture() {
# still requires to be tuned, replaning takes too much time..
# DS 23/11/2017
function gotoWPRRT(transf) {
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
homegps.lat = position.latitude
homegps.long = position.longitude
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1)
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
print(" has to move ", math.vec2.length(rc_goal), "from ", m_pos.x, " ", m_pos.y)
if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5)
@ -104,35 +103,36 @@ function gotoWPRRT(transf) {
else {
if(Path==nil){
# create the map
if(map==nil)
if(map==nil) {
UAVinit_map(rc_goal)
homegps.lat = position.latitude
homegps.long = position.longitude
}
# add proximity sensor and neighbor obstacles to the map
while(Path==nil) {
#getproxobs(m_pos)
UAVgetneiobs (m_pos)
updateMap(m_pos)
Path = UAVpathPlanner(rc_goal, m_pos)
}
cur_goal_l = 1
} else if(cur_goal_l <= size(Path)) {
cur_gps=getvec(Path,cur_goal_l) #x=latitude, y=longitude
cur_pt=vec_from_gps(cur_gps.x,cur_gps.y)
print(" heading to ", cur_pt.x,cur_pt.y)
if(math.vec2.length(cur_pt)>GOTODIST_TOL) {
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1)
UAVgetneiobs(m_pos)
if(check_path(Path,cur_goal_l,m_pos,0)) {
var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude
var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0)
print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y)
if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) {
updateMap(m_pos)
if(check_path(Path, cur_goal_l, m_pos, 0)) {
uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude)
Path = nil
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
while(Path == nil) {
#getproxobs(m_pos)
UAVgetneiobs (m_pos)
updateMap(m_pos)
Path = UAVpathPlanner(rc_goal, m_pos)
}
cur_goal_l = 1
} else {
cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt))
uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude-position.altitude)
}
cur_pt = math.vec2.scale(cur_pt, GOTO_MAXVEL/math.vec2.length(cur_pt))
uav_moveto(cur_pt.x, cur_pt.y, rc_goto.altitude-position.altitude)
}
else
cur_goal_l = cur_goal_l + 1
@ -262,9 +262,13 @@ function LimitAngle(angle){
return angle
}
function vec_from_gps(lat, lon) {
function vec_from_gps(lat, lon, home_ref) {
d_lon = lon - position.longitude
d_lat = lat - position.latitude
if(home_ref == 1) {
d_lon = lon - homegps.long
d_lat = lat - homegps.lat
}
ned_x = d_lat/180*math.pi * 6371000.0;
ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
#Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);

View File

@ -1,4 +1,3 @@
include "vec2.bzz"
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.
@ -6,15 +5,18 @@ include "vstigenv.bzz"
function action() {
statef=action
uav_storegoal(45.5085,-73.1531935979886,5.0)
set_goto(picture)
uav_storegoal(45.5088103899,-73.1540826153,25.0)
set_goto(idle)
}
# Executed once at init time.
function init() {
statef=turnedoff
UAVSTATE = "TURNEDOFF"
uav_initstig()
uav_initswarm()
#statef=turnedoff
#UAVSTATE = "TURNEDOFF"
statef = takeoff
UAVSTATE = "TAKEOFF"
}
# Executed at each time step.

View File

@ -2,7 +2,6 @@ include "vec2.bzz"
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=80 (auto-increment up) with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "vstigenv.bzz"
function action() {
uav_storegoal(-1.0,-1.0,-1.0)
@ -13,7 +12,6 @@ function action() {
function init() {
statef=turnedoff
UAVSTATE = "TURNEDOFF"
uav_initstig()
TARGET_ALTITUDE = 15.0
}