def draw_axis(self, axis, color):
ticks = self._p._axis_ticks[axis]
radius = self._p._tick_length / 2.0
if len(ticks) < 2:
return
# calculate the vector for this axis
axis_lines = [[0, 0, 0], [0, 0, 0]]
axis_lines[0][axis], axis_lines[1][axis] = ticks[0], ticks[-1]
axis_vector = vec_sub(axis_lines[1], axis_lines[0])
# calculate angle to the z direction vector
pos_z = get_direction_vectors()[2]
d = abs(dot_product(axis_vector, pos_z))
d = d / vec_mag(axis_vector)
# don't draw labels if we're looking down the axis
labels_visible = abs(d - 1.0) > 0.02
# draw the ticks and labels
for tick in ticks:
self.draw_tick_line(axis, color, radius, tick, labels_visible)
# draw the axis line and labels
self.draw_axis_line(axis, color, ticks[0], ticks[-1], labels_visible)
def draw_axis(self, axis, color):
ticks = self._p._axis_ticks[axis]
radius = self._p._tick_length / 2.0
if len(ticks) < 2:
return
# calculate the vector for this axis
axis_lines = [[0, 0, 0], [0, 0, 0]]
axis_lines[0][axis], axis_lines[1][axis] = ticks[0], ticks[-1]
axis_vector = vec_sub(axis_lines[1], axis_lines[0])
# calculate angle to the z direction vector
pos_z = get_direction_vectors()[2]
d = abs(dot_product(axis_vector, pos_z))
d = d / vec_mag(axis_vector)
# don't draw labels if we're looking down the axis
labels_visible = abs(d - 1.0) > 0.02
# draw the ticks and labels
for tick in ticks:
self.draw_tick_line(axis, color, radius, tick, labels_visible)
# draw the axis line and labels
self.draw_axis_line(axis, color, ticks[0], ticks[-1], labels_visible)
def run(self):
"""
Run the Vicsek IROS 2014 algorithm thread.
"""
# ============== Vicsek Algorithm -- Parameters ===============
R0 = 12.0 # Pair potential lattice constant (Unit: m)
Dp = 1.0 # Pair potential spring constant (Unit: s^-2)
Cf = 18.0 # Viscous friction coefficient, or damp factor (Unit: m^2/s)
Cs = 5.0 # Viscous friction coefficient for the shill agent (Unit: s^-1)
Vf = 1.0 # Flocking speed for the MAS to maintain (Unit: m/s)
V0 = 2.0 # Maximum tracking velocity
Vmax = 3.0 # Maximum navigating velocity (the copter's total max V)
Alpha = 0.6 # TRG coefficient (0 <= Alpha <= 1)
Beta = 0.8 # COM coefficient (0 <= Beta <= 1)
Rp = 0.5 * R0 # Upper threshold for coping with over-excitation (Short-range)
Rs1 = 0.8 * R0 # Upper threshold incase division by 0 (Mid-range)
Rs2 = 0.3 * R0 # Constant slope factor around R0
Tau = 0.10 # Characteristc time for reaching velocity
Del_t = 0.10 # Feed-forward estimation time
Ts = Del_t # Let sampling period equal to del_t
Rw = 100.0 # Radius for wall (Unit: m)
Rt = 1.0 # Radius for target (Unit: m)
Dw = 5.0 # Transition bandwith for wall (Unit: m)
Ds = 2.0 # Transition bandwith for shape (Unit: m)
Dt = 1.0 # Transition bandwith for target point (Unit: m)
loop_count = 1 # for calculate loop time
average_time = 0
is_on_hold = False # a flag marking if the copter is holding position
no_rendezvous = False # a flag marking if there's a rendezvous point
while not self._stopflag:
time.sleep(0.001)
[self._stopflag,
exit_number] = _check_terminate_condition(self._vehicle, 'Vicsek')
is_on_hold = _check_satellite_low(self._xbee, is_on_hold)
if is_on_hold: continue
loop_begin_time = default_timer() # Flocking algorithm begins
# -------------- <Part I> Repulsion and Viscous -------------- #
friends = copy(self._neighbors)
my_location = self._vehicle.location.global_relative_frame
my_velocity = self._vehicle.velocity
# attributes from Vehicle returns a newly spwaned object, no need to
# `copy`. But the dictionary is mutable so it should be copied.
if shared.rendezvous is None: # hold position if no rendezvous
if not no_rendezvous:
_log_and_broadcast(
self._xbee, "IFO,%s no rendezvous." % shared.AGENT_ID)
no_rendezvous = True
time.sleep(0.5)
continue
else:
no_rendezvous = False
# Self-propelling term
my_speed = util.vec_norm2(my_velocity)
V_spp = util.vec_scal(my_velocity, Vf / my_speed)
# potential term and slipping term
sum_potential = [0.0, 0.0, 0.0]
sum_slipping = [0.0, 0.0, 0.0]
# go thru each neighbor
for j in friends:
# location error (form: [dNorth, dEast, dDown], unit: meters)
# distance, metres (get_distance_metres is also ok)
X_ij = nav.get_position_error(
my_location, friends[j].location_global_relative)
D_ij = util.vec_norm2(
X_ij[0:2]) # only measure ground distance, ignore altitude
# velocity error (form: [dVx, dVy, dVz], unit: m/s)
V_ij = util.vec_sub(my_velocity, friends[j].velocity)
# i. short-range repulsion, when D_ij < R0
if D_ij < R0:
temp_vector = util.vec_scal(X_ij,
min(Rp, R0 - D_ij) / D_ij)
sum_potential = util.vec_add(sum_potential, temp_vector)
# ii. mid-range viscous friction
temp_vector = util.vec_scal(
V_ij, 1.0 / pow(max(Rs1, D_ij - R0 + Rs2), 2))
sum_slipping = util.vec_add(sum_slipping, temp_vector)
# End of for j in friends
# ---- collective potential acceleration ----
A_potential = util.vec_scal(sum_potential, -Dp)
A_slipping = util.vec_scal(sum_slipping, Cf)
# ------------ <Part II> Global Positional Constraint --------------
# alias for the rendezvous point (a comm.WrappedData object)
X_trg = shared.rendezvous.location_global_relative
# i. Flocking: Shill agent wall
X_tmp = nav.get_position_error(my_location, X_trg)
D_tmp = util.vec_norm2(X_tmp[0:2])
# normally the X_trg cannot equal to my_location, but there is still room
# to add algorithm twitches to improve robustness.
# The force of the wall only applies when agents are out of the area
# A_wall = Cs * bump(D_tmp, Rw, Dw) * (Vf/D_tmp*X_tmp - V_i)
temp_vector = util.vec_scal(X_tmp, Vf / D_tmp)
temp_vector = util.vec_sub(my_velocity, temp_vector)
A_wall = util.vec_scal(temp_vector,
Cs * bump_function(D_tmp, Rw, Dw))
# ii. Formation: Shape shift and CoM
[X_shp, X_CoM, Rs] = get_shape_factor(my_location, friends, R0,
'grid')
X_tmp = nav.get_position_error(my_location, X_shp)
D_tmp = util.vec_norm2(X_tmp[0:2])
if D_tmp: # deals with divide-by-zero
V_shp = util.vec_scal(
X_tmp,
Beta * V0 * bump_function(D_tmp, Rs, Ds) / D_tmp)
else:
V_shp = util.vec_scal(X_tmp, 0)
# rendezvous target vector
X_tmp = nav.get_position_error(X_CoM, X_trg)
D_tmp = util.vec_norm2(X_tmp[0:2])
V_trg = util.vec_scal(
X_tmp,
Alpha * V0 * bump_function(D_tmp, Rt, Dt) / D_tmp)
# normally the X_trg cannot equal to X_CoM, but there is still room
# to add algorithm twitches to improve robustness
# tracking vector (normalize if saturated)
V_trk = util.vec_add(V_shp, V_trg)
S_trk = util.vec_norm2(V_trk)
if S_trk > V0: V_trk = util.vec_scal(V_trk, V0 / S_trk)
# ------------ <Part III> Full Dynamic ------------
# V(t+1) = V(t) + 1/Tau*(V_spp + V_trk - V(t))*Del_t + (A_pot + A_slip + A_wall)*del_t
acceleration = util.vec_add(A_wall,
util.vec_add(A_potential, A_slipping))
velocity = util.vec_add(V_spp, util.vec_sub(my_velocity, V_trk))
acceleration = util.vec_scal(acceleration, Del_t)
velocity = util.vec_scal(velocity, Del_t / Tau)
inc_velocity = util.vec_add(acceleration, velocity)
des_velocity = util.vec_add(my_velocity, inc_velocity)
des_speed = util.vec_norm2(des_velocity)
if des_speed > Vmax:
des_velocity = util.vec_scal(des_velocity, Vmax / des_speed)
loop_end_time = default_timer() # Time marker ends
elapsed_time = (loop_end_time -
loop_begin_time) * 1000000 # convert into us
inv_count = 1.0 / loop_count # incremental averaging method
average_time = (
1.0 - inv_count) * average_time + inv_count * elapsed_time
loop_count = loop_count + 1
# Send thru Mavlink
nav.send_ned_velocity(
self._vehicle,
des_velocity[0],
des_velocity[1],
0, # Vz is ignored
Ts)
# End of While #
_log_and_broadcast(
self._xbee, "IFO,%s Vicsek ended with number %d." %
(shared.AGENT_ID, exit_number))
util.log_info("Average loop time: %d us" % average_time)