From b039fcfaccf78511cf22a9d8dc7e253a18be8bc1 Mon Sep 17 00:00:00 2001 From: rafaelrgb Date: Fri, 26 Oct 2018 19:26:07 -0300 Subject: [PATCH 1/2] added new lca version --- buzz_scripts/include/act/states.bzz | 120 +++++++++++++++++++++++++--- 1 file changed, 110 insertions(+), 10 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 01fda7f..7a960a8 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -210,17 +210,117 @@ function formation() { goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0) } +# Lightweight collision avoidance +function LCA_rafael( vel_vec ) { + var safety_radius = 3.0 + collide = 0 + + var k_v = 5 # x axis gain + var k_w = 5 # y axis gain + + cart = neighbors.map( + function(rid, data) { + var c = {} + c.distance = data.distance + c.azimuth = data.azimuth + if (c.distance < (safety_radius * 2.0) ) + collide = 1 + return c + }) + if (collide) { + log("") + log("------> AVOIDING NEIGHBOR! <------") + log("") + result = cart.reduce(function(rid, data, accum) { + if(data.distance < accum.distance and data.distance > 0.0){ + accum.distance = data.distance + accum.angle = data.azimuth + return accum + } + return accum + }, {.distance= safety_radius * 2.0, .angle= 0.0}) + + + d_i = result.distance + data_alpha_i = result.angle + + delta = math.vec2.new( d_i * math.cos(data_alpha_i), d_i * math.sin(data_alpha_i) ) + + p = math.exp(-(d_i - safety_radius)) + if ( math.cos( math.vec2.angle(vel_vec) - data_alpha_i ) < 0.0 ) { + p = math.exp(d_i - safety_radius) + } + + V = -1 * (p / d_i) * k_v * delta.x + W = -1 * (p / d_i) * k_w * delta.y + + Uavd = math.vec2.new( V, W ) + + return math.vec2.add( vel_vec, Uavd ) + + } else + return vel_vec +} + # Custom state function for the developer to play with function cusfun(){ BVMSTATE="CUSFUN" - log("cusfun: yay!!!") - LOCAL_TARGET = math.vec2.new(5 ,0 ) - local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location - if(math.vec2.length(local_set_point) > GOTODIST_TOL){ - local_set_point = LimitSpeed(local_set_point, 1.0) - goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0) - } - else{ - log("TARGET REACHED") - } + #log("cusfun: yay!!!") + #LOCAL_TARGET = math.vec2.new(5 ,0 ) + #local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location + #if(math.vec2.length(local_set_point) > GOTODIST_TOL){ + # local_set_point = LimitSpeed(local_set_point, 1.0) + # goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0) + #} + #else{ + # log("TARGET REACHED") + #} + + initialPoint = 0 + + # Get waypoint list + if (id == 1) { + waypoints = { + .0 = math.vec2.new(-5.0, -5.0), + .1 = math.vec2.new(5.0, 5.0) + } + } else if (id == 2 ) { + waypoints = { + .0 = math.vec2.new(5.0, 5.0), + .1 = math.vec2.new(-5.0, -5.0) + } + } + + # Current waypoint + target_point = waypoints[point] + + # Get drone position and transform to global frame - depends on lauch file + offset_x = 0.0 + offset_y = 0.0 + if (id == 1) { + offset_x = 5.0 + offset_y = 5.0 + } else if (id == 2 ) { + offset_x = -5.0 + offset_y = -5.0 + } + pos_x = pose.position.x + offset_x; + pos_y = pose.position.y + offset_y; + + # Get a vector pointing to target and target distance + vector_to_target = math.vec2.sub(target_point, math.vec2.new(pos_x, pos_y)) + distance_to_target = math.vec2.length(vector_to_target) + + # Run LCA and increment waypoint + if(distance_to_target > 0.2){ + vector_to_target = LCA_rafael(vector_to_target) + vector_to_target = LimitSpeed(vector_to_target, 0.5) + goto_abs(vector_to_target.x, vector_to_target.y, 0.0, 0.0) + } else { + if(point < size(waypoints) - 1){ + point = point + 1 + } else { + point = 0 + } + } } \ No newline at end of file From a6a41309499c1022ae2cd560be3d34f8a05617ee Mon Sep 17 00:00:00 2001 From: rafaelrgb Date: Fri, 26 Oct 2018 19:32:11 -0300 Subject: [PATCH 2/2] moved the lca code to the right file --- buzz_scripts/include/act/CA.bzz | 26 ++++++++------ buzz_scripts/include/act/states.bzz | 54 +---------------------------- 2 files changed, 17 insertions(+), 63 deletions(-) diff --git a/buzz_scripts/include/act/CA.bzz b/buzz_scripts/include/act/CA.bzz index 6d923f0..ed0d055 100644 --- a/buzz_scripts/include/act/CA.bzz +++ b/buzz_scripts/include/act/CA.bzz @@ -1,10 +1,10 @@ -function LCA(vel_vec) { +# Lightweight collision avoidance +function LCA( vel_vec ) { var safety_radius = 3.0 - var default_velocity = 2.0 collide = 0 - var k_v = 5 # velocity gain - var k_w = 10 # angular velocity gain + var k_v = 5 # x axis gain + var k_w = 5 # y axis gain cart = neighbors.map( function(rid, data) { @@ -28,18 +28,24 @@ function LCA(vel_vec) { return accum }, {.distance= safety_radius * 2.0, .angle= 0.0}) + d_i = result.distance data_alpha_i = result.angle - penalty = math.exp(d_i - safety_radius) - if( math.cos(math.vec2.angle(vel_vec)) < 0.0){ - penalty = math.exp(-(d_i - safety_radius)) + delta = math.vec2.new( d_i * math.cos(data_alpha_i), d_i * math.sin(data_alpha_i) ) + + p = math.exp(-(d_i - safety_radius)) + if ( math.cos( math.vec2.angle(vel_vec) - data_alpha_i ) < 0.0 ) { + p = math.exp(d_i - safety_radius) } - V = math.vec2.length(vel_vec) - math.cos(math.vec2.angle(vel_vec)) * penalty * k_v - W = -k_w * penalty * math.sin( math.vec2.angle(vel_vec) ) + V = -1 * (p / d_i) * k_v * delta.x + W = -1 * (p / d_i) * k_w * delta.y + + Uavd = math.vec2.new( V, W ) + + return math.vec2.add( vel_vec, Uavd ) - return math.vec2.newp(V, W) } else return vel_vec } \ No newline at end of file diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 7a960a8..789381e 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -210,58 +210,6 @@ function formation() { goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0) } -# Lightweight collision avoidance -function LCA_rafael( vel_vec ) { - var safety_radius = 3.0 - collide = 0 - - var k_v = 5 # x axis gain - var k_w = 5 # y axis gain - - cart = neighbors.map( - function(rid, data) { - var c = {} - c.distance = data.distance - c.azimuth = data.azimuth - if (c.distance < (safety_radius * 2.0) ) - collide = 1 - return c - }) - if (collide) { - log("") - log("------> AVOIDING NEIGHBOR! <------") - log("") - result = cart.reduce(function(rid, data, accum) { - if(data.distance < accum.distance and data.distance > 0.0){ - accum.distance = data.distance - accum.angle = data.azimuth - return accum - } - return accum - }, {.distance= safety_radius * 2.0, .angle= 0.0}) - - - d_i = result.distance - data_alpha_i = result.angle - - delta = math.vec2.new( d_i * math.cos(data_alpha_i), d_i * math.sin(data_alpha_i) ) - - p = math.exp(-(d_i - safety_radius)) - if ( math.cos( math.vec2.angle(vel_vec) - data_alpha_i ) < 0.0 ) { - p = math.exp(d_i - safety_radius) - } - - V = -1 * (p / d_i) * k_v * delta.x - W = -1 * (p / d_i) * k_w * delta.y - - Uavd = math.vec2.new( V, W ) - - return math.vec2.add( vel_vec, Uavd ) - - } else - return vel_vec -} - # Custom state function for the developer to play with function cusfun(){ BVMSTATE="CUSFUN" @@ -313,7 +261,7 @@ function cusfun(){ # Run LCA and increment waypoint if(distance_to_target > 0.2){ - vector_to_target = LCA_rafael(vector_to_target) + vector_to_target = LCA(vector_to_target) vector_to_target = LimitSpeed(vector_to_target, 0.5) goto_abs(vector_to_target.x, vector_to_target.y, 0.0, 0.0) } else {