Related
I'm trying to simulate a vehicle using a dynamic bicycle model but I cannot seem to get it working. If I set a constant steering angle the lateral velocity grows exponentially and creates impossible results.
a = 0.34284
b = 0.40716
m = 155
I = 37.29
def f_DynBkMdl(x, y, delta, theta, dt, states):
dtheta = states[0]
vlat = states[1]
vlon = states[2]
if delta > math.radians(180):
delta = delta - math.radians(360)
if delta<0:
j = 1
else:
j = 0
if dtheta<0:
q = 1
else:
q = 0
dtheta = abs(dtheta)
delta = abs(delta)
sf = delta - (a*dtheta)/vlon
ff = 30.77*math.degrees(sf)
pf = 0
sr = (b*dtheta)/vlon
fr = 30.77*math.degrees(sr)
pr = 0
if j == 1:
fr = -fr
ff = -ff
if q == 1:
dtheta = -dtheta
theta = theta%math.radians(360)
ddtheta = (a*pf*delta + a*ff - b*fr)/I
dvlat = (pf*delta + ff + fr)/m - vlon*dtheta
dvlon = (pf + pr - ff*delta)/m - vlat*dtheta
dx = -vlat*np.sin(theta) + vlon*np.cos(theta)
dy = vlat*np.cos(theta) + vlon*np.sin(theta)
theta = theta + dtheta*dt + (1/2)*ddtheta*dt**2
dtheta = dtheta + ddtheta*dt
vlat = vlat + dvlat*dt
vlon = vlon + dvlon*dt
vabs = np.sqrt(vlat**2 + vlon**2)
x = x + dx*dt
y = y + dy*dt
states = [dtheta, vlat, vlon]
array = np.array([x, y, theta, vabs, states])
return array
With a and b being the distance between the front and rear axle to the vehicle's centre of gravity, m being the mass and I the inertia. x and y are the global position and theta is the heading with delta being the steering angle. I got these equations from this document. Please note I used a simplified tyre model to calculate the lateral forces and I assumed infinite friction so the friction circle was not needed.
Is there something I am missing to make this work?
I am trying to produce an algorithm where multiple agents (blue) work together as a team to capture a slightly faster enemy agent (red) by preforming surrounding and circling tactics in a 2D grid. So I am trying to make a robust multi-agent algorithm that would allow multi-agents would capture an intelligent and faster enemy agent
So I attempted to give the enemy agent navigation and obstacle avoidance abilities by using something known as potential field navigation. Basically, the enemy agent pretends there is an attraction force at the exit and a repulsive force by each blue agent
Click here for more details on potential fields
When I implemented this into the enemy agent, the agent being attracted to the exit is successful (except for the fact it slows down when it is close to it). The main problem I am having is with the repulsion field where the enemy is trying to avoid the blue particles. While it attempts to escape, it does things such as moving in a zig-zag pattern rapidly, run around a blue particle or group or particles in circles.
I would like the enemy agent to smoothly avoid the blue particles in an intelligent way.
Also, is someone knows how to turn repulsive field into a tangential field, that would be great. I would like tangential fields so that the red particle can slip through the blue particles.
Also, although the code is long, the only function that the enemy agent uses is goToExit(), so that function and any function that it calls will be the only ones relevant.
My code:
import numpy as np
from matplotlib import pyplot as plt
from matplotlib import animation
from random import randint
import random
import math
keep = False
keepX = 0
keepY = 0
### Variables that we can play with ###
interestPointVisual = False
huntEnemy = True
numOfAgents = 10
enemyTopSpeed = 0.5
topSpeed = 0.3
secondDoor = False
resultVisual = False
#obstacleAvoidance = False
chargeEnemy = True
maxFrame = 2000
agentRadius = 2
####################################
phaseCount = 0
fig = plt.figure()
fig.set_dpi(100)
fig.set_size_inches(5, 4.5)
# Declaring the enemy and ally agents
ax = plt.axes(xlim=(0, 100), ylim=(0, 100))
enemy = plt.Circle((10, -10), 0.95, fc='r')
agent = plt.Circle((10, -10), 0.95, fc='b')
if interestPointVisual:
interestColor = 'y'
interestSize = 0.55
else:
interestColor = 'w'
interestSize = 0.55
#interestSize = 0.000001
midpoint = plt.Circle((10, -10), interestSize, fc=interestColor)
eastpoint = plt.Circle((10, -10), interestSize, fc=interestColor)
northpoint = plt.Circle((10, -10), interestSize, fc=interestColor)
westpoint = plt.Circle((10, -10), interestSize, fc=interestColor)
northeastpoint = plt.Circle((10, -10), interestSize, fc=interestColor)
mideastpoint = plt.Circle((10, -10), interestSize, fc=interestColor)
midwestpoint = plt.Circle((10, -10), interestSize, fc=interestColor)
northwestpoint = plt.Circle((10, -10), interestSize, fc=interestColor)
# Adding the exits
rect_size = 5
x_se_s = 47
x_se = 50
y_se = 0
southExit = plt.Rectangle([x_se_s - rect_size / 2, y_se - rect_size / 2], rect_size + 3, rect_size -2 , facecolor='black', edgecolor='black')
x_ne = 50
y_ne = 101
if secondDoor:
northExit = plt.Rectangle([x_ne - rect_size / 2, y_ne - rect_size / 2], rect_size + 3, rect_size -2 , facecolor='black', edgecolor='black')
patches_ac = []
if interestPointVisual:
ax.add_patch(midpoint)
ax.add_patch(northpoint)
ax.add_patch(eastpoint)
ax.add_patch(westpoint)
ax.add_patch(mideastpoint)
ax.add_patch(midwestpoint)
ax.add_patch(northeastpoint)
ax.add_patch(northwestpoint)
# enemy, north, east, south, west
# 0 represents unoccupied, 1 represent occupied
global occupied_ar
global victory
global agentID
global timeStep
global agentLocationAR
ax.add_patch(agent)
for x in range(0, numOfAgents - 1):
agent_clone = plt.Circle((10, -10), 0.95, fc='b')
agent_clone.center = (random.randint(1, 100), random.randint(1, 100))
patches_ac.append(agent_clone)
ax.add_patch(agent_clone)
ax.add_patch(enemy)
# Adding exit patches
ax.add_patch(southExit)
if secondDoor:
ax.add_patch(northExit)
def victoryCheck(enemy_patch):
global agentLocationAR
ex, ey = enemy_patch.center
rangeVal = 0.8
for i in range(0, numOfAgents-1):
if abs(float(ex-agentLocationAR[i][0])) < rangeVal and abs(float(ey-agentLocationAR[i][1])) < rangeVal:
return True
return False
def enemyWonCheck(enemy_patch):
x,y = enemy_patch.center
if (x > x_se - 4 and x < x_se + 4) and y <= y_se +4:
return True
return False
def borderCheck(x,y):
if x < 0:
x = 0
elif x > 100:
x = 100
if y < 0:
y = 0
elif y > 100:
y = 100
return x, y
def init():
global occupied_ar
global agentLocationAR
global keep
global keepX
global keepY
keep = False
keepX = 0
keepY = 0
#enemy.center = (50, 50)
enemy.center = (random.randint(1, 100), random.randint(40, 100))
agent.center = (random.randint(1, 100), random.randint(1, 100))
occupied_ar = np.zeros([9])
agentLocationAR = np.zeros((numOfAgents,2))
for ac in patches_ac:
ac.center = (random.randint(1, 100), random.randint(1, 100))
return []
def animationManage(i):
global occupied_ar
global agentLocationAR
global victory
global agentID
global timeStep
global phaseCount
global maxFrame
timeStep = i
agentID = 1
followTarget(i, agent, enemy)
agentLocationAR[agentID-1][0], agentLocationAR[agentID-1][1] = agent.center
for ac in patches_ac:
agentID = agentID + 1
followTarget(i, ac, enemy)
agentLocationAR[agentID-1][0], agentLocationAR[agentID-1][1] = ac.center
goToExit(i, enemy, southExit)
# printing tests
if victoryCheck(enemy):
print 'Phase ', phaseCount
print 'Victory!'
phaseCount += 1
init()
elif enemyWonCheck(enemy):
print 'Phase ', phaseCount
print 'Failure!'
init()
elif i >= maxFrame - 1:
print 'Phase ', phaseCount
phaseCount += 1
return []
def goToExit(i, patch, exit_patch):
global agentLocationAR
global keep
global keepX
global keepY
x, y = patch.center
v_x, v_y = velocity_calc_exit(patch, exit_patch)
mid_x, mid_y, rad_x, rad_y = getMidDistance(patch, exit_patch)
rad_size = math.sqrt(rad_x**2 + rad_y**2)
v_ax, v_ay = attractionFieldExit(patch, x_se, y_se)
v_rx, v_ry = repulsiveFieldEnemy(patch, 5)
v_x = v_ax + v_rx
v_y = v_ay + v_ry
'''
if abs(v_rx) > 1:
v_x = v_x/abs(v_x/10)
if abs(v_ry) > 1:
v_y = v_x/abs(v_x/10)
'''
# Nomalize the magnitude
v_x = v_x*enemyTopSpeed*0.03
v_y = v_y*enemyTopSpeed*0.03
'''
if abs(v_x) > 1 or abs(v_y) > 1:
print '-------------'
print 'Att X: ', v_ax
print 'Att Y: ', v_ay
print 'Rep X: ', v_rx
print 'Rep Y: ', v_ry
print 'Total X: ', v_x
print 'Total Y: ', v_y
'''
# x position
x += v_x*enemyTopSpeed
# y position
y += v_y*enemyTopSpeed
x,y = borderCheck(x,y)
patch.center = (x, y)
return patch,
def dispersalCalc(user_patch):
global agentLocationAR # we need location of agents
for i in range(0,numOfAgents-1):
if(checkSemiRadius(user_patch, agentRadius)):
return True
return False
def attractionFieldExit(user_patch, attr_x, attr_y):
x,y = user_patch.center
netX = (x - attr_x)
netY = (y - attr_y)
# To prevent slow down when enemy is close to exit
if x - attr_x > 20 or y - attr_y > 20:
if x - attr_x > 20:
netX = (x - attr_x)
else:
if x - attr_x == 0:
netX = 0
else:
netX = 5*((x - attr_x)/abs((x - attr_x)))
if y - attr_y > 30:
netY = (y - attr_y)
else:
if y -attr_y == 0:
netY = 0
else:
netY = 50*((y - attr_y)/abs((y - attr_y)))
#print 'something y ', netY
return -netX, -netY
def repulsiveFieldEnemy(user_patch, repulseRadius):
# repulsive field that will be used by the enemy agent
global agentLocationAR
x,y = user_patch.center
totalRepX = 0
totalRepY = 0
scaleConstant = 1**38
for i in range(0, numOfAgents-1):
repX = 0
repY = 0
avoidX = agentLocationAR[i][0]
avoidY = agentLocationAR[i][1]
# To check if one of the agents to avoid are in range
#print getDistanceScalar(x, y, avoidX, avoidY)
if getDistanceScalar(x, y, avoidX, avoidY) <= repulseRadius:
#print 'Enemy agent detected'
netX = int(x - avoidX)
netY = int(y - avoidY)
# To deal with division by zero and normaize magnitude of repX and repY
if netX == 0:
netX = 0.2*((x - avoidX)/abs(x - avoidX))
if netY == 0:
netY = 0.2*((x - avoidX)/abs(x - avoidX))
repX = ((1/abs(netX)) - (1/repulseRadius))*(netX/(abs(netX)**3))
repY = ((1/abs(netY)) - (1/repulseRadius))*(netY/(abs(netY)**3))
totalRepX = totalRepX + repX
totalRepY = totalRepY + repY
totalRepX = totalRepX/scaleConstant
totalRepY = totalRepY/scaleConstant
return -totalRepX, -totalRepY
def followTarget(i, patch, enemy_patch):
x, y = patch.center
# Will try to follow enemy
#v_x, v_y = velocity_calc(patch, enemy_patch)
# Will follow midpoint of enemy & exit
v_x, v_y = velocity_calc_mid(patch, enemy_patch)
#print 'Here:'
#print interest_ar
# x position
x += v_x
# y position
y += v_y
patch.center = (x, y)
return patches_ac
def getInterestPoints(enemy_patch, exit_patch):
# Calculate interest points to attract agents
x, y = enemy_patch.center
# Calculate enemy-to-exit midpoint
mid_x, mid_y, rad_x, rad_y = getMidDistance(enemy_patch, exit_patch)
interest_ar = np.array([[x,y],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0]])
#north
interest_ar[1][0] = x - rad_x
interest_ar[1][1] = y - rad_y
#east
interest_ar[3][0] = x - rad_y
interest_ar[3][1] = y + rad_x
#south (basically the midpoint)
interest_ar[5][0] = x + rad_x
interest_ar[5][1] = y + rad_y
#west
interest_ar[7][0] = x + rad_y
interest_ar[7][1] = y - rad_x
# northeast
interest_ar[2][0] = (interest_ar[1][0] + interest_ar[3][0])/2
interest_ar[2][1] = (interest_ar[1][1] + interest_ar[3][1])/2
#southeast
interest_ar[4][0] = (interest_ar[3][0] + interest_ar[5][0])/2
interest_ar[4][1] = (interest_ar[3][1] + interest_ar[5][1])/2
#southwest
interest_ar[6][0] = (interest_ar[5][0] + interest_ar[7][0])/2
interest_ar[6][1] = (interest_ar[5][1] + interest_ar[7][1])/2
interest_ar[8][0] = (interest_ar[7][0] + interest_ar[1][0])/2
interest_ar[8][1] = (interest_ar[7][1] + interest_ar[1][1])/2
# Setting up visuals
northpoint.center = (interest_ar[1][0], interest_ar[1][1])
eastpoint.center = (interest_ar[3][0], interest_ar[3][1])
midpoint.center = (interest_ar[5][0], interest_ar[5][1])
westpoint.center = (interest_ar[7][0], interest_ar[7][1])
mideastpoint.center = (interest_ar[2][0], interest_ar[2][1])
midwestpoint.center = (interest_ar[4][0], interest_ar[4][1])
northeastpoint.center = (interest_ar[6][0], interest_ar[6][1])
northwestpoint.center = (interest_ar[8][0], interest_ar[8][1])
return interest_ar
def findClosestInterest(agent_patch, in_ar):
# For some reason, north never gets occupied
# north east is (north/2) + (south/2)
global occupied_ar
global victory
global agentID
global timeStep
global huntEnemy
victory = False
index = -1
smallDis = 999999
tempAr = np.zeros([9])
if huntEnemy:
minDis = 0
else:
minDis = 1
# To check agent's distance of all interest points
for i in range(minDis,9):
dis = abs(int(getDistance(agent_patch, in_ar, i)))
# Add heavy weights to charge at enemy
if chargeEnemy:
if i == 0:
dis = dis*0.5
if occupied_ar[i] != 0:
# we must write a condition so that agent knows it is the
# one that is occupying it
dis = dis*5
# Add heavy weights to avoid the back
if i == 1 or i == 8 or i == 2:
if i == 1:
dis = dis*3
elif i == 2 or i == 8:
dis = dis*4
tempAr[i] = dis
# When we discover unoccupied shorter distance, replace index
if dis < smallDis:
# index is agent_patch.center[0] < 47 and agent_patch.center[0] > 53the index of interest_array of the closest interest point
smallDis = dis
index = i
# If the smallest distance is less than 10, we are currently engaged
if smallDis < 0.5:
# We are near or at the targeted interest point,
# now we should update array as occupied
occupied_ar[index] = agentID
if occupied_ar[0] != 0:
victory = True
#print 'engaged index ', index
else:
# Else we are still far away from the index
if occupied_ar[index] == agentID:
occupied_ar[index] = 0
#print 'lost track of index ', index
#else:
#print 'far away from index ', index
return index
def getBypassInterestPoints(user_patch,avoidX, avoidY, exit_x, exit_y):
# Mainly used by the enemy agent
# User agent will find a point around the blocking agent that is closest to
# the exit.
x,y = user_patch.center
rad_range = 20
tempX = x - avoidX
tempY = y - avoidY
diffR = math.sqrt(tempX**2 + tempY**2)
# Calculating our target x and y length
radX = (rad_range*tempX)/diffR
radY = (rad_range*tempY)/diffR
# Now we calculate the main interest points
# Since we are calculating perpendicular points, we reverse the X and Y
# in the pt calculation process
pt1X = avoidX + radY
pt1Y = avoidY - radX
###
pt2X = avoidX - radY
pt2Y = avoidY + radX
# Then we must determine which interest point is closer to the exit
pt1Dis = int(getDistanceScalar(pt1X, pt1Y,exit_x, exit_y))
pt2Dis = int(getDistanceScalar(pt2X, pt2Y,exit_x, exit_y))
# If point 1 is closer to the exit than point 2
if(int(pt1Dis) <= int(pt2Dis)):
print int(pt1X)
return pt1X, pt1Y
print int(pt2X)
return int(pt2X), int(pt2Y)
def getDistanceScalar(x1, y1, x2, y2):
return math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
def getDistance(agent_patch, in_ar, index):
x_a, y_a = agent_patch.center
x_t = in_ar[index][0]
y_t = in_ar[index][1]
# get distance between two particles
ans = math.sqrt((x_t - x_a)**2 + (y_t - y_a)**2)
if math.isnan(ans):
print 'x_a: ',x_a
print 'y_a: ',y_a
print 'x_t: ',x_t
print 'y_t: ',y_t
init()
return math.sqrt((x_t - x_a)**2 + (y_t - y_a)**2)
def getMidDistance(enemy_patch, exit_patch):
# Get midpoint between enemy agent and exit
x, y = enemy_patch.center
x_e = x_se
y_e = y_se
# Get midpoint values
mid_x = (x + x_e)/2
mid_y = (y + y_e)/2
# Get radius values
rad_x = mid_x - x
rad_y = mid_y - y
# Returns (midpoint x and y) values and (radius x and y) values
return mid_x, mid_y, rad_x, rad_y
def top_speed_regulate(curr_speed, top_speed):
if curr_speed > top_speed:
return top_speed
elif curr_speed < -top_speed:
return -top_speed
else:
return curr_speed
def velocityCalcScalar(x1, y1, x2, y2):
veloX = top_speed_regulate( (x2 - x1) ,enemyTopSpeed)
veloY = top_speed_regulate( (y2 - y1) ,enemyTopSpeed)
return veloX, veloY
# Calculate velocity to rush to exit
def velocity_calc_exit(agent_patch, exit_patch):
x, y = agent_patch.center
#x_e, y_e = exit_patch.center
x_e = x_se
y_e = y_se
velo_vect = np.array([0.0, 0.0], dtype='f')
dis_limit_thresh = 1
velo_vect[0] = top_speed_regulate( (x_e - x)* dis_limit_thresh ,enemyTopSpeed)
velo_vect[1] = top_speed_regulate( (y_e - y)* dis_limit_thresh ,enemyTopSpeed)
return velo_vect[0], velo_vect[1]
# Calculate velocity to chase down enemy
def velocity_calc(agent_patch, enemy_patch):
x, y = agent_patch.center
x_e, y_e = enemy_patch.center
velo_vect = np.array([0.0, 0.0], dtype='f')
dis_limit_thresh = 1
velo_vect[0] = top_speed_regulate( (x_e - x)* dis_limit_thresh ,topSpeed)
velo_vect[1] = top_speed_regulate( (y_e - y)* dis_limit_thresh ,topSpeed)
return velo_vect[0], velo_vect[1]
# Calculate velocity to arrive at midpoint between enemy and exit
def velocity_calc_mid(agent_patch, enemy_patch):
x, y = agent_patch.center
x_e, y_e, _, _ = getMidDistance(enemy_patch, southExit)
# We get location of interest points as well as animate the interest points
interest_ar = getInterestPoints(enemy_patch, southExit)
interest_index = findClosestInterest(agent_patch, interest_ar)
x_e = interest_ar[interest_index][0]
y_e = interest_ar[interest_index][1]
velo_vect = np.array([0.0, 0.0], dtype='f')
dis_limit_thresh = 1
topSpeed = 0.3
velo_vect[0] = top_speed_regulate( (x_e - x)* dis_limit_thresh , topSpeed)
velo_vect[1] = top_speed_regulate( (y_e - y)* dis_limit_thresh , topSpeed)
'''
if dispersalCalc(agent_patch):
velo_vect[0] = 0
velo_vect[1] = 0
'''
return velo_vect[0], velo_vect[1]
def checkRadius(user_patch, r):
global agentLocationAR
r = 1
for i in range(0,numOfAgents-1):
x = int(agentLocationAR[i][0])
y = int(agentLocationAR[i][1])
if(inRadius(user_patch, x, y, r)):
# if an agent is in the user's radius
#print 'Nearby agent detected'
return True
return False
def checkSemiRadius(user_patch, r):
global agentLocationAR
r = 0.001
for i in range(0,numOfAgents-1):
x = int(agentLocationAR[i][0])
y = int(agentLocationAR[i][1])
if(inSemiRadius(user_patch, x, y, r)):
# if an agent is in the user's radius
#print 'Nearby agent detected'
return True
return False
def inRadius(self_patch, pointX, pointY, r):
# Helps determine if there is something near the using agent
x, y = self_patch.center # agent emitting the radius
# agent we are trying to avoid
h = pointX
k = pointY
# Equation of circle
# (x-h)^2 + (y-k)^2 <= r^2
tempX = (x - h)**2
tempY = (y - k)**2
r_2 = r**2
if tempX + tempY <= r_2:
# It is within the radius
return True
else:
return False
def inSemiRadius(self_patch, pointX, pointY, r):
# Helps determine if there is something near the using agent
h, k = self_patch.center # agent emitting the radius
# agent we are trying to avoid
x = pointX
y = pointY
# Equation of semicircle
tempTerm = r**2 - (x-h)**2
if tempTerm < 0:
# if this term is negative, that means agent to avoid is out of range
return False
tempEq = k - math.sqrt(tempTerm)
if y <= tempEq:
# It is within the radius
return True
else:
return False
def animateCos(i, patch):
x, y = patch.center
x += 0.1
y = 50 + 30 * np.cos(np.radians(i))
patch.center = (x, y)
return patch,
anim = animation.FuncAnimation(fig, animationManage,
init_func=init,
frames=maxFrame,
interval=1,
blit=True,
repeat=True)
plt.show()
I am trying to implement 4th order Runge Kutta for nonholonomic motion for car-like robots.
I don't know what I am doing wrong,essentially I am passing +-Pi/4 to calculate hard left and right turns to get different trajectories.
But no matter if I pass +pi/4 or -pi/4 to it, I get the same answer.
I cannot figure out what I am doing wrong.
The constraint equations that I am using are:
thetadot = (s/L)*tan(phi)
xdot = s*cos(theta)
ydot = s*sin(theta)
Where s is the speed and L is the length of the car like robot.
#! /usr/bin/env python
import sys, random, math, pygame
from pygame.locals import *
from math import sqrt,cos,sin,atan2,tan
import numpy as np
import matplotlib.pyplot as plt
XDIM = 640
YDIM = 480
WINSIZE = [XDIM, YDIM]
PHI = 45
s = 0.5
white = 255, 240, 200
black = 20, 20, 40
red = 255, 0, 0
green = 0, 255, 0
blue = 0, 0, 255
cyan = 0,255,255
pygame.init()
screen = pygame.display.set_mode(WINSIZE)
X = XDIM/2
Y = YDIM/2
THETA = 45
def main():
nodes = []
nodes.append(Node(XDIM/2.0,YDIM/2.0,0.0))
plt.plot(runge_kutta(nodes[0], (3.14/4))) #Hard Left turn
plt.plot(runge_kutta(nodes[0], 0)) #Straight ahead
plt.plot(runge_kutta(nodes[0], -(3.14/4))) #Hard Right turn
plt.show()
class Node:
x = 0
y = 0
theta = 0
distance=0
parent=None
def __init__(self,xcoord, ycoord, theta):
self.x = xcoord
self.y = ycoord
self.theta = theta
def rk4(f, x, y, n):
x0 = y0 = 0
vx = [0]*(n + 1)
vy = [0]*(n + 1)
h = 0.8
vx[0] = x = x0
vy[0] = y = y0
for i in range(1, n + 1):
k1 = h*f(x, y)
k2 = h*f(x + 0.5*h, y + 0.5*k1)
k3 = h*f(x + 0.5*h, y + 0.5*k2)
k4 = h*f(x + h, y + k3)
vx[i] = x = x0 + i*h
vy[i] = y = y + (k1 + k2 + k2 + k3 + k3 + k4)/6
print "1"
print vy
return vy
def fun1(x,y):
x = (0.5/2)*tan(y)
print "2"
print x
return x
def fun2(x,y):
x = 0.5*cos(y)
print "3"
print x
return x
def fun3(x,y):
x = 0.5*sin(y)
print "4"
print x
return x
def runge_kutta(p, phi):
x1 = p.x
y1 = p.y
theta1 = p.theta
fi = phi
for i in range(0,5):
x2 = rk4(fun2, x1, theta1, 5)
y2 = rk4(fun3, y1, theta1, 5)
theta2 = rk4(fun1, theta1 ,fi, 5)
theta1 = theta2
print "5"
print zip(x2,y2)
return zip(x2,y2)
# if python says run, then we should run
if __name__ == '__main__':
main()
running = True
while running:
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
I can't really say much about the algorithm, but the way you set up our rk4 function, the x, and y arguments will never have any effect:
def rk4(f, x, y, n):
x0 = y0 = 0 # x0 and y0 will both be 0 after this
vx = [0]*(n + 1)
vy = [0]*(n + 1)
h = 0.8
vx[0] = x = x0 # now x will be 0
vy[0] = y = y0 # and y will be 0 too
...
The rest of the function will use x=0 and y=0 in any case.
Also, I don't know if that's intentional, but the other functions fun1, fun2 and fun3 don't ever use the parameter passed as x, they only use y. They change x locally, but that won't reflect outside the function.
I would like to make random mouse movements in specified rectangle area (limited with coordinates x1, y1, x2, y2, x3, y3, x4, y4).
Movements should be smooth, random, not just straight lines, go randomly up/down/left/right/etc for specified time duration.
Could you give me a hand or working example I can learn from?
many thanks
This code works on Windows only. You can experiment with the parameters inside the random_movement function to get better results. Good luck!
import ctypes
import random
import time
import math
def move_mouse(pos):
x_pos, y_pos = pos
screen_size = ctypes.windll.user32.GetSystemMetrics(0), ctypes.windll.user32.GetSystemMetrics(1)
x = 65536L * x_pos / screen_size[0] + 1
y = 65536L * y_pos / screen_size[1] + 1
return ctypes.windll.user32.mouse_event(32769, x, y, 0, 0)
def random_movement(top_left_corner, bottom_right_corner, min_speed=100, max_speed=200):
'''speed is in pixels per second'''
x_bound = top_left_corner[0], bottom_right_corner[0]
y_bound = top_left_corner[1], bottom_right_corner[1]
pos = (random.randrange(*x_bound),
random.randrange(*y_bound))
speed = min_speed + random.random()*(max_speed-min_speed)
direction = 2*math.pi*random.random()
def get_new_val(min_val, max_val, val, delta=0.01):
new_val = val + random.randrange(-1,2)*(max_val-min_val)*delta
if new_val<min_val or new_val>max_val:
return get_new_val(min_val, max_val, val, delta)
return new_val
steps_per_second = 35.0
while True:
move_mouse(pos)
time.sleep(1.0/steps_per_second)
speed = get_new_val(min_speed, max_speed, speed)
direction+=random.randrange(-1,2)*math.pi/5.0*random.random()
new_pos = (int(round(pos[0]+speed*math.cos(direction)/steps_per_second)),
int(round(pos[1]+speed*math.sin(direction)/steps_per_second)))
while new_pos[0] not in xrange(*x_bound) or new_pos[1] not in xrange(*y_bound):
direction = 2*math.pi*random.random()
new_pos = (int(round(pos[0]+speed*math.cos(direction)/steps_per_second)),
int(round(pos[1]+speed*math.sin(direction)/steps_per_second)))
pos=new_pos
Example:
random_movement((300,300),(600,600))
For random smooth movements constrained to a rectangle I'd try to use Lissajous curves with randomly changing coefficients.
Here is a demo of random X,Y positions you can play as your requirements:
from time import sleep
import pyautogui
import numpy as np
# Check your screen size
print(pyautogui.size())
count=0
while count<1000:
x=np.random.randint(1,1792)
y=np.random.randint(1,1120)
pyautogui.moveTo(x, y)
print(x)
print(y)
sleep(20)
count+=1
Note: install first
pip3 install pyautogui
I made this based on Piotr Dabkowski's code, with some extra features (taking random breaks, random scrolls, and users can end early by right clicking). This works for Python 3, and again, for Windows only.
import ctypes
import random
import time
import math
import win32gui
xmax = ctypes.windll.user32.GetSystemMetrics(0)
ymax = ctypes.windll.user32.GetSystemMetrics(1)
def get_position():
_, _, (x,y) = win32gui.GetCursorInfo()
return (x,y)
def move_mouse(pos):
x_pos, y_pos = pos
x = int(65536 * x_pos / xmax + 1)
y = int(65536 * y_pos / ymax + 1)
return ctypes.windll.user32.mouse_event(32769, x, y, 0, 0)
def start(t=30, min_speed=10, max_speed=500, x_bound=[0,xmax], y_bound=[0,ymax],
p_break = 0.005, break_range = (10, 60), p_scroll = 0.01, scroll_range = (100, 1000)):
def get_new_speed(min_val, max_val, val, delta=0.01):
new_val = val + random.randrange(-1,2)*(max_val-min_val)*delta
if new_val<min_val or new_val>max_val:
return get_new_speed(min_val, max_val, val, delta)
return new_val
steps_per_second = 35.0
print('Started.')
endtime = time.time() + int(t*60)
# Initialize position, speed and direction
pos = get_position()
speed = min_speed + random.random()*(max_speed-min_speed)
direction = 2*math.pi*random.random()
inside_boundary = False
right_clicked = False
# Keep moving mouse until end time, or until right click
while (not right_clicked) and (time.time() < endtime):
if ctypes.windll.user32.GetKeyState(0x02) not in [0,1]:
right_clicked = True
time.sleep(1.0/steps_per_second)
# Taking a break of random duration
duration = random.randint(*break_range) # in unit of seconds
break_endtime = time.time() + duration
r = random.random()
if (1-p_break) <= r < 1:
# Keep checking for right click to exit loop
while (not right_clicked) and (time.time() < break_endtime):
if ctypes.windll.user32.GetKeyState(0x02) not in [0,1]:
right_clicked = True
time.sleep(1.0/steps_per_second)
time_left = break_endtime - time.time()
print('Paused %d / %ds' % (time_left,duration) + ' '*50, end='\r')
pos = get_position()
print(' '*50, end='\r')
# Random scroll
r = random.random()
lines = random.randint(*scroll_range)
sign = random.random()
sign = -1 if sign < 0.5 else 1
if (1-p_scroll) <= r < 1:
time.sleep(random.random())
ctypes.windll.user32.mouse_event(2048, 0, 0, sign*lines, 0)
time.sleep(random.random())
pos = get_position()
# Random move
move_mouse(pos)
time_left = endtime - time.time()
print('Running (time left: %ds)' % time_left + ' '*50, end='\r')
if (pos[0] in range(*x_bound)) and (pos[1] in range(*y_bound)):
inside_boundary = True
# Update position, speed and direction
speed = get_new_speed(min_speed, max_speed, speed)
direction+=random.randrange(-1,2)*math.pi/5.0*random.random()
new_pos = (int(round(pos[0]+speed*math.cos(direction)/steps_per_second)),
int(round(pos[1]+speed*math.sin(direction)/steps_per_second)))
# Once mouse position is inside boundary, new position must also be inside
if inside_boundary:
while new_pos[0] not in range(*x_bound) or new_pos[1] not in range(*y_bound):
direction = 2*math.pi*random.random()
new_pos = (int(round(pos[0]+speed*math.cos(direction)/steps_per_second)),
int(round(pos[1]+speed*math.sin(direction)/steps_per_second)))
pos=new_pos
print('Stopped.' + ' ' * 50)
For performing the movements there is a third-party package call PyUserInput that will allow you to control mouse or keyboard, and it is cross-platform. Install it using pip:
$ pip install PyUserInput
For doing smooth movements you can try what 9000 proposes in his answer.
Guys I have asked this question before but did not receive a single comment or answer
I want to simulate a search algorithm on a power law graph and want to visually see the algorithm move from one node to another on the graph. How do I do that?
You can adapt this completely different code I happen to have written for Find the most points enclosed in a fixed size circle :)
The useful bit is:
It uses the basic windowing system tkinter to create a frame containing a canvas; it then does some algorithm, calling it's own 'draw()' to change the canvas and then 'update()' to redraw the screen, with a delay. From seeing how easy it is to chart in tkinter, you can perhaps move on to interactive versions etc.
import random, math, time
from Tkinter import * # our UI
def sqr(x):
return x*x
class Point:
def __init__(self,x,y):
self.x = float(x)
self.y = float(y)
self.left = 0
self.right = []
def __repr__(self):
return "("+str(self.x)+","+str(self.y)+")"
def distance(self,other):
return math.sqrt(sqr(self.x-other.x)+sqr(self.y-other.y))
def equidist(left,right,dist):
u = (right.x-left.x)
v = (right.y-left.y)
if 0 != u:
r = math.sqrt(sqr(dist)-((sqr(u)+sqr(v))/4.))
theta = math.atan(v/u)
x = left.x+(u/2)-(r*math.sin(theta))
if x < left.x:
x = left.x+(u/2)+(r*math.sin(theta))
y = left.y+(v/2)-(r*math.cos(theta))
else:
y = left.y+(v/2)+(r*math.cos(theta))
else:
theta = math.asin(v/(2*dist))
x = left.x-(dist*math.cos(theta))
y = left.y + (v/2)
return Point(x,y)
class Vis:
def __init__(self):
self.frame = Frame(root)
self.canvas = Canvas(self.frame,bg="white",width=width,height=height)
self.canvas.pack()
self.frame.pack()
self.run()
def run(self):
self.count_calc0 = 0
self.count_calc1 = 0
self.count_calc2 = 0
self.count_calc3 = 0
self.count_calc4 = 0
self.count_calc5 = 0
self.prev_x = 0
self.best = -1
self.best_centre = []
for self.sweep in xrange(0,len(points)):
self.count_calc0 += 1
if len(points[self.sweep].right) <= self.best:
break
self.calc(points[self.sweep])
self.sweep = len(points) # so that draw() stops highlighting it
print "BEST",self.best+1, self.best_centre # count left-most point too
print "counts",self.count_calc0, self.count_calc1,self.count_calc2,self.count_calc3,self.count_calc4,self.count_calc5
self.draw()
def calc(self,p):
for self.right in p.right:
self.count_calc1 += 1
if (self.right.left + len(self.right.right)) < self.best:
# this can never help us
continue
self.count_calc2 += 1
self.centre = equidist(p,self.right,radius)
assert abs(self.centre.distance(p)-self.centre.distance(self.right)) < 1
count = 0
for p2 in p.right:
self.count_calc3 += 1
if self.centre.distance(p2) <= radius:
count += 1
if self.best < count:
self.count_calc4 += 4
self.best = count
self.best_centre = [self.centre]
elif self.best == count:
self.count_calc5 += 5
self.best_centre.append(self.centre)
self.draw()
self.frame.update()
time.sleep(0.1)
def draw(self):
self.canvas.delete(ALL)
# draw best circle
for best in self.best_centre:
self.canvas.create_oval(best.x-radius,best.y-radius,\
best.x+radius+1,best.y+radius+1,fill="red",\
outline="red")
# draw current circle
if self.sweep < len(points):
self.canvas.create_oval(self.centre.x-radius,self.centre.y-radius,\
self.centre.x+radius+1,self.centre.y+radius+1,fill="pink",\
outline="pink")
# draw all the connections
for p in points:
for p2 in p.right:
self.canvas.create_line(p.x,p.y,p2.x,p2.y,fill="lightGray")
# plot visited points
for i in xrange(0,self.sweep):
p = points[i]
self.canvas.create_line(p.x-2,p.y,p.x+3,p.y,fill="blue")
self.canvas.create_line(p.x,p.y-2,p.x,p.y+3,fill="blue")
# plot current point
if self.sweep < len(points):
p = points[self.sweep]
self.canvas.create_line(p.x-2,p.y,p.x+3,p.y,fill="red")
self.canvas.create_line(p.x,p.y-2,p.x,p.y+3,fill="red")
self.canvas.create_line(p.x,p.y,self.right.x,self.right.y,fill="red")
self.canvas.create_line(p.x,p.y,self.centre.x,self.centre.y,fill="cyan")
self.canvas.create_line(self.right.x,self.right.y,self.centre.x,self.centre.y,fill="cyan")
# plot unvisited points
for i in xrange(self.sweep+1,len(points)):
p = points[i]
self.canvas.create_line(p.x-2,p.y,p.x+3,p.y,fill="green")
self.canvas.create_line(p.x,p.y-2,p.x,p.y+3,fill="green")
radius = 60
diameter = radius*2
width = 800
height = 600
points = []
# make some points
for i in xrange(0,100):
points.append(Point(random.randrange(width),random.randrange(height)))
# sort points for find-the-right sweep
points.sort(lambda a, b: int(a.x)-int(b.x))
# work out those points to the right of each point
for i in xrange(0,len(points)):
p = points[i]
for j in xrange(i+1,len(points)):
p2 = points[j]
if p2.x > (p.x+diameter):
break
if (abs(p.y-p2.y) <= diameter) and \
p.distance(p2) < diameter:
p.right.append(p2)
p2.left += 1
# sort points in potential order for sweep, point with most right first
points.sort(lambda a, b: len(b.right)-len(a.right))
# debug
for p in points:
print p, p.left, p.right
# show it
root = Tk()
vis = Vis()
root.mainloop()
You can use matplotlib for that.
Here is a simlple example of a mesh with an animated highlighted point:
import matplotlib.pyplot as plt
import time
x_size = 4
y_size = 3
# create the points and edges of the mesh
points = [(x,y) for y in range(y_size) for x in range(x_size)]
vert_edges = [((i_y*x_size)+i_x,(i_y*x_size)+i_x+1)
for i_x in range(x_size-1) for i_y in range(y_size)]
horz_edges = [((i_y*x_size)+i_x,((i_y+1)*x_size)+i_x)
for i_x in range(x_size) for i_y in range(y_size-1)]
edges = vert_edges + horz_edges
# plot all the points and edges
lines = []
for edge in edges:
x_coords, y_coords = zip(points[edge[0]], points[edge[1]])
lines.extend((x_coords, y_coords, 'g'))
plt.plot(linewidth=1, *lines)
x, y = zip(*points)
plt.plot(x, y, 'o')
# create the highlighted point
point_plot = plt.plot([0], [0], 'ro')[0]
# turn on interactive plotting mode
plt.ion()
plt.ylim(-1, y_size)
plt.xlim(-1, x_size)
# animate the highlighted point
for i_point in range(1, len(x)):
point_plot.set_xdata([x[i_point]])
point_plot.set_ydata([y[i_point]])
plt.draw()
time.sleep(0.5)
plt.show()