def electron_rep_prim(self,u,v,j,k):
yp = u.exp + v.exp
yq = j.exp + k.exp
P = vec_add(vec_times(u.centre,u.exp/yp),vec_times(v.centre,v.exp/yp))
Q = vec_add(vec_times(j.centre,j.exp/yq),vec_times(k.centre,k.exp/yq))
AA = vec_dot(u.centre,u.centre)
BB = vec_dot(v.centre,v.centre)
AB = vec_dot(u.centre,v.centre)
CC = vec_dot(j.centre,j.centre)
DD = vec_dot(k.centre,k.centre)
CD = vec_dot(j.centre,k.centre)
K1 =exp((-1*u.exp * v.exp*(AA -2*AB + BB))/(yp))
K2 =exp((-1*j.exp * k.exp*(CC -2*CD + DD))/(yq))
pq = vec_minus(P,Q)
PQ = vec_dot(pq,pq)
n1 = (2*u.exp/pi)**0.75
n2 = (2*v.exp/pi)**0.75
n3 = (2*j.exp/pi)**0.75
n4 = (2*k.exp/pi)**0.75
E = 2*pi**2.5*K1*K2/(yp*yq*(yp+yq)**0.5) * self.f0(PQ*yp*yq/(yp+yq)) * u.coef*v.coef*j.coef*k.coef * n1*n2*n3*n4
return E
def I(self,u,v,var): #var = x,y,z
##print '--------------- I',var,' ----------------'
I_sum = 0
y = u.exp + v.exp
P = vec_add(scal_dot(u.centre,u.exp/y),scal_dot(v.centre,v.exp/y))
if (var == 'x'):
l1=u.l
l2=v.l
PA = P[0]-u.centre[0]
PB = P[0]-v.centre[0]
elif(var=='y'):
l1=u.m
l2=v.m
PA = P[1]-u.centre[1]
PB = P[1]-v.centre[1]
elif(var=='z'):
l1=u.n
l2=v.n
PA = P[2]-u.centre[2]
PB = P[2]-v.centre[2]
# PA = dot(z.vec_minus(P,u.centre),z.vec_minus(P,u.centre))
# PB=dot(z.vec_minus(P,v.centre),z.vec_minus(P,v.centre))
##print 'I',var,' looping to',(l1+l2)/2
for i in xrange((l1+l2)/2 + 1) :
# #print 'i=',i
I_sum+=self.f_k(u,v,2*i,PA,PB,l1,l2) * (self.double_fac(2*i-1))/((2*y)**i) * (pi/y)**0.5
return I_sum
def tick(self):
# TODO beautify method (make some lines shorter)
# Cancel movement, if hero is standing
if self.state in [Hero.LEFT_STAND, Hero.RIGHT_STAND,
Hero.UP_STAND, Hero.DOWN_STAND]:
return
# Otherwise: Determine direction
direction = {
Hero.LEFT_WALK : Hero.LEFT,
Hero.RIGHT_WALK : Hero.RIGHT,
Hero.UP_WALK : Hero.UP,
Hero.DOWN_WALK : Hero.DOWN
}[self.state]
corners = self.get_corners(direction)
delta = util.vec_mult(direction, self.speed)
dest_tiles = [self.levelmap.pixel_pos_to_tile_pos(
util.vec_add(c, delta)) for c in corners]
tiletypes =\
[self.levelmap.get_tile_type(t) == LevelMap.LevelMap.FLOOR
for t in dest_tiles]
if reduce(lambda a, b: a and b, tiletypes):
self.pos.add(delta)
else:
self.move_to_edge(direction)
def get_corners(self, direction):
upper_left = self.pos
upper_right = util.vec_add(self.pos, Vector(self.size-1, 0))
lower_left = util.vec_add(self.pos, Vector(0, self.size-1))
lower_right = util.vec_add(self.pos, Vector(self.size-1, self.size-1))
if direction == Hero.LEFT:
return [upper_left, lower_left]
elif direction == Hero.RIGHT:
return [upper_right, lower_right]
elif direction == Hero.UP:
return [upper_left, upper_right]
elif direction == Hero.DOWN:
return [lower_left, lower_right]
else:
return []
def show(self, screen, offset):
pos = util.vec_add(self.pos, offset)
offset_pos = (pos.x, pos.y - 16)
if self.state == Hero.DOWN_WALK:
self.__walk_down.show(screen, pos)
elif self.state == Hero.UP_WALK:
self.__walk_up.show(screen, pos)
elif self.state == Hero.LEFT_WALK:
self.__walk_left.show(screen, pos)
elif self.state == Hero.RIGHT_WALK:
self.__walk_right.show(screen, pos)
elif self.state == Hero.DOWN_STAND:
screen.blit(self.__img, offset_pos, (4, 0, 16, 31))
elif self.state == Hero.UP_STAND:
screen.blit(self.__img, offset_pos, (4, 32, 16, 31))
elif self.state == Hero.LEFT_STAND:
screen.blit(self.__img, offset_pos, (4, 64, 16, 31))
elif self.state == Hero.RIGHT_STAND:
screen.blit(self.__img, offset_pos, (4, 96, 16, 31))
def prim_nuclear(self,u,v,C,name):
charges = {'h':1,'he':2,'c':6,'o':8}
y = u.exp + v.exp
#AA = my_dot(u.centre,u.centre)
#BB = my_dot(v.centre,v.centre)
#AB = my_dot(u.centre,v.centre)
R1 = vec_minus(u.centre,v.centre)
R2 = my_dot(R1,R1)
P = vec_add(scal_dot(u.centre,u.exp/y),scal_dot(v.centre,v.exp/y))
PC = dist(P,C)
Z_c = charges[name]#1.2266/(0.87622069)
if(P[0]==C[0] and P[1]==C[1] and P[2]==C[2]):
B=Z_c
# #print 'P==C'
else:
# #print 'P!=C'
B = Z_c * (pi**0.5*erf(y**0.5*PC))/(2*y**0.5 * PC)
V =-1*(2*pi*exp((-1*u.exp * v.exp*(R2))/(y)))/(y)*B * u.coef*v.coef *((2*u.exp/pi)**0.75) * ((2*v.exp/pi)**0.75 )
return V
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)
def read_zmatrix(self,fname):
f = open(fname,'r')
self.file = f;
name = f.readline().strip()
self.mol = Molecule()
self.mol.add_atom(Atom(name.lower(),1,0,0,0,connect=0));
str = f.readline();
sp = str.split();
if (sp == []):
return self.mol
sp[0] = sp[0].lower()
if (sp==[]):
return self.mol
try:
bl = float(sp[2])
except:
bl = self.lookup_var(sp[2])
self.mol.add_atom(Atom(sp[0],2,0,0,bl,int(sp[1])))
self.mol.add_bond(2,int(sp[1]))
str = f.readline();
sp = str.split();
if (sp == []):
return self.mol
sp[0] = sp[0].lower()
try:
bl = float(sp[2]);
except:
bl = self.lookup_var(sp[2])
try:
ba = float(sp[4]);
except:
ba = self.lookup_var(sp[4])
a = Atom(sp[0],3,math.sin(ba*math.pi/180.0)*bl,0,self.mol.atoms[int(sp[1]) -1].z - math.cos(ba*math.pi/180.0)*bl,int(sp[1]))# BIG CHANGE
self.mol.add_atom(a)
self.mol.add_bond(3,int(sp[1]))
atom_number = 4;
while (True):
#print atom_number
str = f.readline();
sp = str.split();
if (sp==[]):
print 'end of matrix'
break
sp[0] = sp[0].lower()
try:
bl = float(sp[2]);
except:
bl = self.lookup_var(sp[2])
try:
ba = float(sp[4]);
except:
ba = self.lookup_var(sp[4])
try:
dh = float(sp[6]);
except:
dh = self.lookup_var(sp[6])
connect = int(sp[1])
angle_connect = int(sp[3])
dihed_connect = int(sp[5])
atoms = self.mol.atoms;
#print 'connect: %i angle_connect: %i' %(connect,angle_connect)
vector1 = vec_minus(atoms[connect-1].xyz,atoms[angle_connect-1].xyz );
vector2 = vec_minus(atoms[connect-1].xyz,atoms[dihed_connect-1].xyz );
norm1 = vec_cross(vector1,vector2)
norm2 = vec_cross(vector1,norm1)
norm1 = normalise(norm1)
norm2 = normalise(norm2)
norm1 =vec_times(norm1,-1*math.sin(dh*math.pi/180))
norm2 = vec_times(norm2,math.cos(dh*math.pi/180))
vector3 =vec_add(norm1,norm2)
vector3 =normalise(vector3)
vector3 = vec_times(vector3,bl*math.sin(ba*math.pi/180.0))
vector1 = normalise(vector1)
vector1 = vec_times(vector1,bl*math.cos(ba*math.pi/180.0))
vector2 = vec_add(atoms[connect - 1].xyz,vector3)
vector2 = vec_minus(vector2,vector1)
a = Atom(sp[0],atom_number,vector2[0],vector2[1],vector2[2],int(sp[1]))
self.mol.add_atom(a)
self.mol.add_bond(atom_number,int(sp[1]))
atom_number+=1;
return self.mol
