Develop Reference

Object values not being reset in python function

Here is my code. In the calculateOptimalLambda() function, I am attempting to declare a copy of n and
store it as m, remove one point from m, and make some calculations and a graph. Then, the loop should
restart, make a fresh copy of m, remove the next point, and so on.
However, when in the next iteration
of the loop, a point has been removed. Eventually, I run out of points to remove, and I get an error.
How do I declare a fresh copy of m so I can remove the next point?
import numpy as np
from matplotlib import pyplot as plt
class Data:
def __init__(self, points, sigma, lamda):
self.points = points
self.sigma = sigma
self.sample = np.random.uniform(-1,1, (points, 2))
self.transformedData = np.ones((points, 5))
self.weight = np.zeros((5,1))
self.lamda = lamda
def changeLamda(self,x):
self.lamda = x
def removePoint(self, x):
self.points = self.points - 1
self.sample = np.delete(self.sample, x, 0)
self.transformedData = np.delete(self.transformedData, x, 0)
def transformedFunction(self, x):
transformedData = np.ones((1, 5))
transformedData[0,1] = x
transformedData[0,2] = 0.5 * (3*x**2 -1)
transformedData[0,3]= 0.5 * (5*x**3 - 3*x)
transformedData[0,4] = 0.125 * (35*x**4 -30*x**2 + 3)
return np.dot(transformedData, self.weight)
def setY(self):
for i in range(len(self.sample[0:,0])):
self.sample[i,1] = np.random.normal(0, self.sigma) + self.sample[i,0]**2
def transform(self):
for i in range(len(self.sample[0:,0])):
self.transformedData[i,1] = self.sample[i,0]
self.transformedData[i,2] = 0.5 * (3*self.sample[i,0]**2 -1)
self.transformedData[i,3]= 0.5 * (5*self.sample[i,0]**3 - 3*self.sample[i,0])
self.transformedData[i,4] = 0.125 * (35*self.sample[i,0]**4 -30*self.sample[i,0]**2 + 3)
def calculateWeight(self):
z = n.transformedData
zProd = np.linalg.inv(np.matmul(np.transpose(z), z) + np.identity(5)*self.lamda)
next1 = np.matmul(zProd,np.transpose(z))
a = self.sample[0:,1]
a = a.reshape((-1, 1))
print(a)
self.weight = np.matmul(next1,a)
def calculateError(self):
error= (np.matmul(self.transformedData, self.weight) - self.sample[1,0:])
return error/self.points
def calculateOptimalLambda(n, L):
a = 0
for i in range(len(L)):
n.changeLamda(L[i])
for x in range(n.getPoints()):
a+=1
plt.subplot(4,5,a)
m = n
m.removePoint(x)
m.calculateWeight()
weight = m.getWeight()
error = m.calculateError()
twoD_plot(m)
print(error)
def twoD_plot(n):
t = np.linspace(-1, 1, 400)
x = np.square(t)
plt.plot(t,x,'b')
error = 0
y = x
for i in range(len(t)):
y[i] = n.transformedFunction(t[i])
error += (y[i] - t[i]**2)**2
"""print(error/len(t))"""
plt.plot(t,y,'r')
plt.scatter(n.getSample()[0:,0],n.getSample()[0:,1], c = 'g', marker = 'o')
n = Data(5,0.1,0)
n.setY()
n.transform()
n.calculateWeight()
L = [1, 0.01, 0.00001, 0]
calculateOptimalLambda(n, L)
plt.show()

I cannot get my 3 DOF bicycle model to work

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?

Trying to implement potential field navigation in matplotlib

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()

What is wrong with my Implementation of 4th Order runge kutta in python for nonholonomic constraints?

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.

Mesh Generation for Computational Science in Python

I have a need for a Python module/package that provides a mesh on which I can do computational science? I am not doing graphics, so I don't think the blender package is what I want.
Does anyone know of a good package?

The most useful packages out there are perhaps
mshr,
pygalmesh,
dmsh,
pygmsh, and
MeshPy,
meshzoo.
In addition, there is optimesh for improving the quality of any mesh.
(Disclaimer: I'm the author of pygmsh, pygalmesh, dmsh, meshzoo, and optimesh.)

If you're trying to solve FE or CFD style equations on a mesh you can use MeshPy in 2 and 3 dimensions. Meshpy is a nice wrapper around the existing tools tetgen and triangle.
If you're looking for more typical graphics style meshes, there was an interesting talk at PyCon 2011 "Algorithmic Generation of OpenGL Geometry", which described a pragmatic approach to procedural mesh generation. The code from the presentation is available online
If you're interested in reconstruction of surfaces from data, you can't go past the Standford 3D Scanning Repository, home of the Stanford Bunny
Edit:
A dependancy free alternative may be to use something like gmsh, which is platform independent, and uses similar tools to meshpy in its back-end.

I recommend using NumPy (especially if you've used MATLAB before). Many computational scientists / mechanical engineers working in python might agree, but I'm biased as it found it's way into much of the last year of my research. It's part of SciPy: http://numpy.scipy.org/
I was fond of numpy.linspace(a,b,N) which makes an N length vector of equally spaced values from a to b. You can use numpy.ndarray to make a N x M matrix, or if you want 2D arrays use numpy.meshgrid.

Here is code adapted from Kardontchik's port,
import numpy as np
from numpy import pi as pi
from scipy.spatial import Delaunay
import matplotlib.pylab as plt
from scipy.optimize import fmin
import matplotlib.pylab as plt
def ktrimesh(p,bars,pflag=0):
# create the (x,y) data for the plot
xx1 = p[bars[:,0],0]; yy1 = p[bars[:,0],1]
xx2 = p[bars[:,1],0]; yy2 = p[bars[:,1],1]
xmin = np.min(p[:,0])
xmax = np.max(p[:,0])
ymin = np.min(p[:,1])
ymax = np.max(p[:,1])
xmin = xmin - 0.05*(xmax - xmin)
xmax = xmax + 0.05*(xmax - xmin)
ymin = ymin - 0.05*(ymax - ymin)
ymax = ymax + 0.05*(ymax - ymin)
plt.figure()
for i in range(len(xx1)):
xp = np.array([xx1[i],xx2[i]])
yp = np.array([yy1[i],yy2[i]])
plt.plot(xmin,ymin,'.',xmax,ymax,'.',markersize=0.1)
plt.plot(xp,yp,'k')
plt.axis('equal')
if pflag == 0:
stitle = 'Triangular Mesh'
if pflag == 1:
stitle = 'Visual Boundary Integrity Check'
#plt.title('Triangular Mesh')
plt.title(stitle)
plt.xlabel('x')
plt.ylabel('y')
plt.show()
return 1
def ccw_tri(p,t):
"""
orients all the triangles counterclockwise
"""
# vector A from vertex 0 to vertex 1
# vector B from vertex 0 to vertex 2
A01x = p[t[:,1],0] - p[t[:,0],0]
A01y = p[t[:,1],1] - p[t[:,0],1]
B02x = p[t[:,2],0] - p[t[:,0],0]
B02y = p[t[:,2],1] - p[t[:,0],1]
# if vertex 2 lies to the left of vector A the component z of
# their vectorial product A^B is positive
Cz = A01x*B02y - A01y*B02x
a = t[np.where(Cz<0)]
b = t[np.where(Cz>=0)]
a[:,[1,2]] = a[:,[2,1]]
t = np.concatenate((a, b))
return t
def triqual_flag(p,t):
# a(1,0), b(2,0), c(2,1)
a = np.sqrt((p[t[:,1],0] - p[t[:,0],0])**2 + (p[t[:,1],1] - p[t[:,0],1])**2)
b = np.sqrt((p[t[:,2],0] - p[t[:,0],0])**2 + (p[t[:,2],1] - p[t[:,0],1])**2)
c = np.sqrt((p[t[:,2],0] - p[t[:,1],0])**2 + (p[t[:,2],1] - p[t[:,1],1])**2)
A = 0.25*np.sqrt((a+b+c)*(b+c-a)*(a+c-b)*(a+b-c))
R = 0.25*(a*b*c)/A
r = 0.5*np.sqrt( (a+b-c)*(b+c-a)*(a+c-b)/(a+b+c) )
q = 2.0*(r/R)
min_edge = np.minimum(np.minimum(a,b),c)
min_angle_deg = (180.0/np.pi)*np.arcsin(0.5*min_edge/R)
min_q = np.min(q)
min_ang = np.min(min_angle_deg)
return min_q, min_ang
def triqual(p,t,fh,qlim=0.2):
# a(1,0), b(2,0), c(2,1)
a = np.sqrt((p[t[:,1],0] - p[t[:,0],0])**2 + (p[t[:,1],1] - p[t[:,0],1])**2)
b = np.sqrt((p[t[:,2],0] - p[t[:,0],0])**2 + (p[t[:,2],1] - p[t[:,0],1])**2)
c = np.sqrt((p[t[:,2],0] - p[t[:,1],0])**2 + (p[t[:,2],1] - p[t[:,1],1])**2)
A = 0.25*np.sqrt((a+b+c)*(b+c-a)*(a+c-b)*(a+b-c))
R = 0.25*(a*b*c)/A
r = 0.5*np.sqrt( (a+b-c)*(b+c-a)*(a+c-b)/(a+b+c) )
q = 2.0*(r/R)
pmid = (p[t[:,0]] + p[t[:,1]] + p[t[:,2]])/3.0
hmid = fh(pmid)
Ah = A/hmid
Anorm = Ah/np.mean(Ah)
min_edge = np.minimum(np.minimum(a,b),c)
min_angle_deg = (180.0/np.pi)*np.arcsin(0.5*min_edge/R)
plt.figure()
plt.subplot(3,1,1)
plt.hist(q)
plt.title('Histogram;Triangle Statistics:q-factor,Minimum Angle and Area')
plt.subplot(3,1,2)
plt.hist(min_angle_deg)
plt.ylabel('Number of Triangles')
plt.subplot(3,1,3)
plt.hist(Anorm)
plt.xlabel('Note: for equilateral triangles q = 1 and angle = 60 deg')
plt.show()
indq = np.where(q < qlim) # indq is a tuple: len(indq) = 1
if list(indq[0]) != []:
print ('List of triangles with q < %5.3f and the (x,y) location of their nodes' % qlim)
print ('')
print ('q t[i] t[nodes] [x,y][0] [x,y][1] [x,y][2]')
for i in indq[0]:
print ('%.2f %4d [%4d,%4d,%4d] [%+.2f,%+.2f] [%+.2f,%+.2f] [%+.2f,%+.2f]' % \
(q[i],i,t[i,0],t[i,1],t[i,2],p[t[i,0],0],p[t[i,0],1],p[t[i,1],0],p[t[i,1],1],p[t[i,2],0],p[t[i,2],1]))
print ('')
# end of detailed data on worst offenders
return q,min_angle_deg,Anorm
class Circle:
def __init__(self,xc,yc,r):
self.xc, self.yc, self.r = xc, yc, r
def __call__(self,p):
xc, yc, r = self.xc, self.yc, self.r
d = np.sqrt((p[:,0] - xc)**2 + (p[:,1] - yc)**2) - r
return d
class Rectangle:
def __init__(self,x1,x2,y1,y2):
self.x1, self.x2, self.y1, self.y2 = x1,x2,y1,y2
def __call__(self,p):
x1,x2,y1,y2 = self.x1, self.x2, self.y1, self.y2
d1 = p[:,1] - y1 # if p inside d1 > 0
d2 = y2 - p[:,1] # if p inside d2 > 0
d3 = p[:,0] - x1 # if p inside d3 > 0
d4 = x2 - p[:,0] # if p inside d4 > 0
d = -np.minimum(np.minimum(np.minimum(d1,d2),d3),d4)
return d
class Polygon:
def __init__(self,verts):
self.verts = verts
def __call__(self,p):
verts = self.verts
# close the polygon
cverts = np.zeros((len(verts)+1,2))
cverts[0:-1] = verts
cverts[-1] = verts[0]
# initialize
inside = np.zeros(len(p))
dist = np.zeros(len(p))
Cz = np.zeros(len(verts)) # z-components of the vectorial products
dist_to_edge = np.zeros(len(verts))
in_ref = np.ones(len(verts))
# if np.sign(Cz) == in_ref then point is inside
for j in range(len(p)):
Cz = (cverts[1:,0] - cverts[0:-1,0])*(p[j,1] - cverts[0:-1,1]) - \
(cverts[1:,1] - cverts[0:-1,1])*(p[j,0] - cverts[0:-1,0])
dist_to_edge = Cz/np.sqrt( \
(cverts[1:,0] - cverts[0:-1,0])**2 + \
(cverts[1:,1] - cverts[0:-1,1])**2)
inside[j] = int(np.array_equal(np.sign(Cz),in_ref))
dist[j] = (1 - 2*inside[j])*np.min(np.abs(dist_to_edge))
return dist
class Union:
def __init__(self,fd1,fd2):
self.fd1, self.fd2 = fd1, fd2
def __call__(self,p):
fd1,fd2 = self.fd1, self.fd2
d = np.minimum(fd1(p),fd2(p))
return d
class Diff:
def __init__(self,fd1,fd2):
self.fd1, self.fd2 = fd1, fd2
def __call__(self,p):
fd1,fd2 = self.fd1, self.fd2
d = np.maximum(fd1(p),-fd2(p))
return d
class Intersect:
def __init__(self,fd1,fd2):
self.fd1, self.fd2 = fd1, fd2
def __call__(self,p):
fd1,fd2 = self.fd1, self.fd2
d = np.maximum(fd1(p),fd2(p))
return d
class Protate:
def __init__(self,phi):
self.phi = phi
def __call__(self,p):
phi = self.phi
c = np.cos(phi)
s = np.sin(phi)
temp = np.copy(p[:,0])
rp = np.copy(p)
rp[:,0] = c*p[:,0] - s*p[:,1]
rp[:,1] = s*temp + c*p[:,1]
return rp
class Pshift:
def __init__(self,x0,y0):
self.x0, self.y0 = x0,y0
def __call__(self,p):
x0, y0 = self.x0, self.y0
p[:,0] = p[:,0] + x0
p[:,1] = p[:,1] + y0
return p
def Ellipse_dist_to_minimize(t,p,xc,yc,a,b):
x = xc + a*np.cos(t) # coord x of the point on the ellipse
y = yc + b*np.sin(t) # coord y of the point on the ellipse
dist = (p[0] - x)**2 + (p[1] - y)**2
return dist
class Ellipse:
def __init__(self,xc,yc,a,b):
self.xc, self.yc, self.a, self.b = xc, yc, a, b
self.t, self.verts = self.pick_points_on_shape()
def pick_points_on_shape(self):
xc, yc, a, b = self.xc, self.yc, self.a, self.b
c = np.array([xc,yc])
t = np.linspace(0,(7.0/4.0)*pi,8)
verts = np.zeros((8,2))
verts[:,0] = c[0] + a*np.cos(t)
verts[:,1] = c[1] + b*np.sin(t)
return t, verts
def inside_ellipse(self,p):
xc, yc, a, b = self.xc, self.yc, self.a, self.b
c = np.array([xc,yc])
r, phase = self.rect_to_polar(p-c)
r_ellipse = self.rellipse(phase)
in_ref = np.ones(len(p))
inside = 0.5 + 0.5*np.sign(r_ellipse-r)
return inside
def rect_to_polar(self,p):
r = np.sqrt(p[:,0]**2 + p[:,1]**2)
phase = np.arctan2(p[:,1],p[:,0])
# note: np.arctan2(y,x) order; phase in +/- pi (+/- 180deg)
return r, phase
def rellipse(self,phi):
a, b = self.a, self.b
r = a*b/np.sqrt((b*np.cos(phi))**2 + (a*np.sin(phi))**2)
return r
def find_closest_vertex(self,point):
t, verts = self.t, self.verts
dist = np.zeros(len(t))
for i in range(len(t)):
dist[i] = (point[0] - verts[i,0])**2 + (point[1] - verts[i,1])**2
ind = np.argmin(dist)
t0 = t[ind]
return t0
def __call__(self,p):
xc, yc, a, b = self.xc, self.yc, self.a, self.b
t, verts = self.t, self.verts
dist = np.zeros(len(p))
inside = self.inside_ellipse(p)
for j in range(len(p)):
t0 = self.find_closest_vertex(p[j]) # initial guess to minimizer
opt = fmin(Ellipse_dist_to_minimize,t0, \
args=(p[j],xc,yc,a,b),full_output=1,disp=0)
# add full_output=1 so we can retrieve the min dist(squared)
# (2nd argument of opt array, 1st argument is the optimum t)
min_dist = np.sqrt(opt[1])
dist[j] = min_dist*(1 - 2*inside[j])
return dist
def distmesh(fd,fh,h0,xmin,ymin,xmax,ymax,pfix,ttol=0.1,dptol=0.001,Iflag=1,qmin=1.0):
geps = 0.001*h0; deltat = 0.2; Fscale = 1.2
deps = h0 * np.sqrt(np.spacing(1))
random_seed = 17
h0x = h0; h0y = h0*np.sqrt(3)/2 # to obtain equilateral triangles
Nx = int(np.floor((xmax - xmin)/h0x))
Ny = int(np.floor((ymax - ymin)/h0y))
x = np.linspace(xmin,xmax,Nx)
y = np.linspace(ymin,ymax,Ny)
# create the grid in the (x,y) plane
xx,yy = np.meshgrid(x,y)
xx[1::2] = xx[1::2] + h0x/2.0 # shifts even rows by h0x/2
p = np.zeros((np.size(xx),2))
p[:,0] = np.reshape(xx,np.size(xx))
p[:,1] = np.reshape(yy,np.size(yy))
p = np.delete(p,np.where(fd(p) > geps),axis=0)
np.random.seed(random_seed)
r0 = 1.0/fh(p)**2
p = np.concatenate((pfix,p[np.random.rand(len(p))<r0/max(r0),:]))
pold = np.inf
Num_of_Delaunay_triangulations = 0
Num_of_Node_movements = 0 # dp = F*dt
while (1):
Num_of_Node_movements += 1
if Iflag == 1 or Iflag == 3: # Newton flag
print ('Num_of_Node_movements = %3d' % (Num_of_Node_movements))
if np.max(np.sqrt(np.sum((p - pold)**2,axis = 1))) > ttol:
Num_of_Delaunay_triangulations += 1
if Iflag == 1 or Iflag == 3: # Delaunay flag
print ('Num_of_Delaunay_triangulations = %3d' % \
(Num_of_Delaunay_triangulations))
pold = p
tri = Delaunay(p) # instantiate a class
t = tri.vertices
pmid = (p[t[:,0]] + p[t[:,1]] + p[t[:,2]])/3.0
t = t[np.where(fd(pmid) < -geps)]
bars = np.concatenate((t[:,[0,1]],t[:,[0,2]], t[:,[1,2]]))
bars = np.unique(np.sort(bars),axis=0)
if Iflag == 4:
min_q, min_angle_deg = triqual_flag(p,t)
print ('Del iter: %3d, min q = %5.2f, min angle = %3.0f deg' \
% (Num_of_Delaunay_triangulations, min_q, min_angle_deg))
if min_q > qmin:
break
if Iflag == 2 or Iflag == 3:
ktrimesh(p,bars)
# move mesh points based on bar lengths L and forces F
barvec = p[bars[:,0],:] - p[bars[:,1],:]
L = np.sqrt(np.sum(barvec**2,axis=1))
hbars = 0.5*(fh(p[bars[:,0],:]) + fh(p[bars[:,1],:]))
L0 = hbars*Fscale*np.sqrt(np.sum(L**2)/np.sum(hbars**2))
F = np.maximum(L0-L,0)
Fvec = np.column_stack((F,F))*(barvec/np.column_stack((L,L)))
Ftot = np.zeros((len(p),2))
n = len(bars)
for j in range(n):
Ftot[bars[j,0],:] += Fvec[j,:] # the : for the (x,y) components
Ftot[bars[j,1],:] -= Fvec[j,:]
# force = 0 at fixed points, so they do not move:
Ftot[0: len(pfix),:] = 0
# update the node positions
p = p + deltat*Ftot
# bring outside points back to the boundary
d = fd(p); ix = d > 0 # find points outside (d > 0)
dpx = np.column_stack((p[ix,0] + deps,p[ix,1]))
dgradx = (fd(dpx) - d[ix])/deps
dpy = np.column_stack((p[ix,0], p[ix,1] + deps))
dgrady = (fd(dpy) - d[ix])/deps
p[ix,:] = p[ix,:] - np.column_stack((dgradx*d[ix], dgrady*d[ix]))
# termination criterium: all interior nodes move less than dptol:
if max(np.sqrt(np.sum(deltat*Ftot[d<-geps,:]**2,axis=1))/h0) < dptol:
break
final_tri = Delaunay(p) # another instantiation of the class
t = final_tri.vertices
pmid = (p[t[:,0]] + p[t[:,1]] + p[t[:,2]])/3.0
# keep the triangles whose geometrical center is inside the shape
t = t[np.where(fd(pmid) < -geps)]
bars = np.concatenate((t[:,[0,1]],t[:,[0,2]], t[:,[1,2]]))
# delete repeated bars
#bars = unique_rows(np.sort(bars))
bars = np.unique(np.sort(bars),axis=0)
# orient all the triangles counterclockwise (ccw)
t = ccw_tri(p,t)
# graphical output of the current mesh
ktrimesh(p,bars)
triqual(p,t,fh)
return p,t,bars
def boundary_bars(t):
# create the bars (edges) of every triangle
bars = np.concatenate((t[:,[0,1]],t[:,[0,2]], t[:,[1,2]]))
# sort all the bars
data = np.sort(bars)
# find the bars that are not repeated
Delaunay_bars = dict()
for row in data:
row = tuple(row)
if row in Delaunay_bars:
Delaunay_bars[row] += 1
else:
Delaunay_bars[row] = 1
# return the keys of Delaunay_bars whose value is 1 (non-repeated bars)
bbars = []
for key in Delaunay_bars:
if Delaunay_bars[key] == 1:
bbars.append(key)
bbars = np.asarray(bbars)
return bbars
def plot_shapes(xc,yc,r):
# circle for plotting
t_cir = np.linspace(0,2*pi)
x_cir = xc + r*np.cos(t_cir)
y_cir = yc + r*np.sin(t_cir)
plt.figure()
plt.plot(x_cir,y_cir)
plt.grid()
plt.title('Shapes')
plt.xlabel('x')
plt.ylabel('y')
plt.axis('equal')
#plt.show()
return
plt.close('all')
xc = 0; yc = 0; r = 1.0
x1,y1 = -1.0,-2.0
x2,y2 = 2.0,3.0
plot_shapes(xc,yc,r)
xmin = -1.5; ymin = -1.5
xmax = 1.5; ymax = 1.5
h0 = 0.4
pfix = np.zeros((0,2)) # null 2D array, no fixed points provided
fd = Circle(xc,yc,r)
fh = lambda p: np.ones(len(p))
p,t,bars = distmesh(fd,fh,h0,xmin,ymin,xmax,ymax,pfix,Iflag=4)