Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
test
import numpy as np
import matplotlib.patches as patches
class BSpline():
def __init__(self):
pass
self.warp = 4.
self.count = 26*2
def process(self, Px, Py, Pw, ax, MODE=1, draw=True):
self.draw = draw
self.mode = MODE # Can be 0 or 1 for terminal tangents
self.ax = ax
self.Px = Px
self.Py = Py
self.Pw = Pw
count = self.count
if self.mode == 0:
self.n = len(Px) - 2
else:
self.n = len(Px) - 2
self.n1 = self.n+1;
self.B0 = [None]*count
self.B1 = [None]*count
self.B2 = [None]*count
self.B3 = [None]*count
self.dx = [None]*self.n
self.dy = [None]*self.n
t = 0.;
self.ts = []
for i in range(0, count):
t1 = 1-t
t12 = t1*t1
t2 = t*t
self.B0[i] = t1*t12
self.B1[i] = 3*t*t12
self.B2[i] = 3*t2*t1
self.B3[i] = t*t2
self.ts.append(t)
t += 1.00/count
return self.drawSpline()
def findCPoints(self):
if self.mode == 0:
self.dx[0] = self.Px[self.n] - self.Px[0];
self.dy[0] = self.Py[self.n] - self.Py[0];
self.dx[self.n-1] = -(self.Px[self.n1] - self.Px[self.n-1])
self.dy[self.n-1] = -(self.Py[self.n1] - self.Py[self.n - 1])
else:
DIV = 3.
self.dx[0] = (self.Px[1] - self.Px[0])/DIV;
self.dy[0] = (self.Py[1] - self.Py[0])/DIV;
self.dx[self.n-1] = (self.Px[self.n-1] - self.Px[self.n-2])/DIV;
self.dy[self.n-1] = (self.Py[self.n-1] - self.Py[self.n-2])/DIV;
#self.warp += 0.05
warps = self.Pw
Ax = [None]*self.n
Ay = [None]*self.n
Bi = [None]*self.n
Bi[1] = -1/warps[1]
Ax[1] = -(self.Px[2] - self.Px[0] - self.dx[0])*Bi[1]
Ay[1] = -(self.Py[2] - self.Py[0] - self.dy[0])*Bi[1];
for i in range(2, self.n-1):
warpval = warps[i]
Bi[i] = -1/(warpval+ Bi[i-1]);
Ax[i] = -(self.Px[i+1] - self.Px[i-1] - Ax[i-1])*Bi[i];
Ay[i] = -(self.Py[i+1] - self.Py[i-1] - Ay[i-1])*Bi[i];
for i in range(self.n-2, 0, -1):
self.dx[i] = Ax[i] + self.dx[i+1]*Bi[i];
self.dy[i] = Ay[i] + self.dy[i+1]*Bi[i];
#self.dx[1] = 0
#self.dy[0] = 0
#self.dy[1] = 0
# for i in range(0, len(self.dx)):
# self.dx[i] = 0
# self.dy[i] = 0
# print self.dx
# print self.dy
def drawSpline(self):
self.findCPoints()
w = 1.
h = 1.
d = 0.
h1 = h
d2 = d
step = 1./w
t = step;
scPx = [None]*self.n
scPy = [None]*self.n
scDx = [None]*self.n
scDy = [None]*self.n
X = None
Y = None
squaresize = 0.3
for i in range(0, self.n):
X = scPx[i] = self.Px[i]*w
Y = scPy[i] = self.Py[i]*h;
scDx[i] = self.dx[i]*w;
scDy[i] = self.dy[i]*h;
if self.draw:
rect = patches.Rectangle((X - squaresize/2.,Y - squaresize/2.),squaresize,squaresize,linewidth=1,edgecolor='r',facecolor='none')
self.ax.add_patch(rect)
#if self.mode == 0:
if self.draw:
self.ax.add_patch(patches.Rectangle((scPx[0]+scDx[0] - squaresize/2., (scPy[0]+scDy[0])- squaresize/2.),squaresize,squaresize,
linewidth=1,edgecolor='b',facecolor='none'))
self.ax.add_patch(patches.Rectangle((scPx[self.n-1]-scDx[self.n-1] - squaresize/2., (scPy[self.n-1]-scDy[self.n-1])- squaresize/2.),squaresize,squaresize,
linewidth=1,edgecolor='b',facecolor='none'))
# if self.n > 1:
# paths = [[scPx[0], scPy[0]]]
# for i in range(1, self.n):
# paths.append([scPx[i-1]+scDx[i-1], scPy[i-1]-scDy[i-1]])
# paths.append([scPx[i]-scDx[i], scPy[i]+scDy[i]])
# paths.append([scPx[i], scPy[i]])
# paths_arr = np.array(paths)
# plt.plot(paths_arr[:, 0], paths_arr[:, 1])
# #for path in paths:
# paths = [[scPx[0], scPy[0]]]
# for i in range(0, self.n-1):
# for k in range(0, self.count):
# X = (scPx[i]*self.B0[k] + (scPx[i] + scDx[i])*self.B1[k] +
# (scPx[i+1] - scDx[i+1])*self.B2[k] + scPx[i+1]*self.B3[k])
# Y = (scPy[i]*self.B0[k] + (scPy[i] + scDy[i])*self.B1[k] +
# (scPy[i+1] - scDy[i+1])*self.B2[k] + scPy[i+1]*self.B3[k]);
# paths.append([X,Y])
# paths_arr = np.array(paths)
# plt.scatter(paths_arr[:, 0], paths_arr[:, 1], s=0.2)
paths = [[scPx[0], scPy[0]]]
for i in range(0, self.n-1):
dist = np.sqrt((self.Px[i]-self.Px[i+1])**2 + (self.Py[i]-self.Py[i+1])**2)
count = int(dist/0.01)
# i want a point every 1cm
for t in np.linspace(0., 1.-(1./count), count):
t1 = 1-t
t12 = t1*t1
t2 = t*t
B0 = t1*t12
B1 = 3*t*t12
B2 = 3*t2*t1
B3 = t*t2
X = (scPx[i]*B0 + (scPx[i] + scDx[i])*B1 +
(scPx[i+1] - scDx[i+1])*B2 + scPx[i+1]*B3)
Y = (scPy[i]*B0 + (scPy[i] + scDy[i])*B1 +
(scPy[i+1] - scDy[i+1])*B2 + scPy[i+1]*B3);
paths.append([X,Y])
paths_arr = np.array(paths)
if self.draw:
plt.scatter(paths_arr[:, 0], paths_arr[:, 1], s=0.2)
return paths_arr
# cv = np.array(
# [[ 2.70967742, 1.30411255, 4. ], # D
# [ 2.45564516, 6.28246753, 4. ], # C
# [-0.90322581, 6.39069264, 4. ],
# [-1.04435484, 1.27705628, 4. ],
# [-5.75806452, 1.41233766, 4. ], # B
# [-5.95564516, 6.14718615, 4. ], # A
# [ 2.51209677, 3.79329004, 4. ], # T: Should be center of CD
# [-6.06854839, 4.11796537, 4. ]] # T: Should be center of AB
# )
# Case that doesnt make sense. A point is completly not imaged cos the
cv = np.array(
[[ 3.21774194, 7.04004329, 4., ],
[ 1.75, 7.17532468, 4., ],
[ 1.07258065, 3.27922078, 4., ],
[-0.56451613, 3.38528139, 4., ],
[-1.60887097, 6.85064935, 4., ],
[-2.87903226, 6.7965368, 4., ],
[-3.86693548, 3.2521645, 4., ],
[ 6.29435484, 5.76623377, 4., ],
[-6.06854839, 6.11796537, 4., ]]
)
cv[:,2] = 5
# cv = np.array([
# [2.33076, 5.08771, 100000000],
# [1.18207, 5.43119, 100000000],
# [0.325818, 4.84136, 100000000],
# [0.269373, 6.14113, 100000000],
# [-2.61584, 5.66069, 100000000],
# [0,0,0],
# [0,0,0]
# ])
# # Alternating points
# cv = np.array(
# [[ 3.38709677, 4.57792208, 100. ],
# [ 1.18207, 5.43119, 100. ],
# [ -1.10080645, 4.17207792, 24. ],
# [ -3.0766129, 5.11904762, 18. ],
# [ -5.44758065, 4.09090909, 100. ],
# [ 0., 0., 100. ],
# [ 0., 0., 100. ]]
# )
# # Exceed bound
# cv = np.array(
# [[ 3.38709677, 4.57792208, 5. ],
# [ 0.56451613, 2.38636364, 2.5 ],
# [-0.79032258, 7.41883117, 2.5 ],
# [-2.90725806, 5.38961039, 4.5 ],
# [-5.44758065, 4.09090909, 5. ],
# [ 0., 0., 5. ],
# [ 0., 0., 5. ]]
# )
# cv[:,2] = 100
# A case to prove my interpolation helps?
cv = np.array(
[[ 3.38709677, 4.57792208, 200. ],
[ 1.3266129, 7.04004329, 1.5 ],
[ -1.15725806, 6.2012987, 24.5 ],
[ -2.625, 5.30844156, 200. ],
[ 0., 0., 200. ],
[ 0., 0., 200. ]]
)
cv[:,2] = 5
# To test optimization to remove two peaks?
cv = np.array(
[[ 3.38709677, 4.57792208, 4.6 ],
[ 1.01612903, 7.71645022, 4.6 ],
[-1.3266129, 7.01298701, 4.6 ],
[-2.625, 5.30844156, 4.6 ],
[ 0., 0., 4.6 ],
[ 0., 0., 4.6 ]]
)
released = False
#cv[:,2] = 100
class Click():
def __init__(self, ax, button=1):
self.ax=ax
self.button=button
self.press=False
self.move = False
self.c1=self.ax.figure.canvas.mpl_connect('button_press_event', self.onpress)
self.c2=self.ax.figure.canvas.mpl_connect('button_release_event', self.onrelease)
self.c3=self.ax.figure.canvas.mpl_connect('motion_notify_event', self.onmove)
self.c4=self.ax.figure.canvas.mpl_connect('scroll_event',self.onzoom)
def onzoom(self, event):
print("zoom")
global cv
lowest_dist = 1000
lowest_index = -1
for i in range(0, cv.shape[0]):
dist = np.sqrt((cv[i,0]-event.xdata)**2 + (cv[i,1]-event.ydata)**2)
if dist < lowest_dist:
lowest_dist = dist
lowest_index = i
if lowest_dist < 2:
if event.button == "up":
cv[lowest_index, 2] += 0.5
elif event.button == "down":
if cv[lowest_index, 2] > 1.5:
cv[lowest_index, 2] -= 0.5
# if event.button == "up":
# cv[:, 2] += 0.5
# elif event.button == "down":
# cv[:, 2] -= 0.5
def onclick(self,event):
if event.inaxes == self.ax:
global cv
lowest_dist = 1000
lowest_index = -1
for i in range(0, cv.shape[0]):
dist = np.sqrt((cv[i,0]-event.xdata)**2 + (cv[i,1]-event.ydata)**2)
if dist < lowest_dist:
lowest_dist = dist
lowest_index = i
if event.button == self.button:
if lowest_dist > 1:
cv = np.vstack((np.array([event.xdata, event.ydata, 4.]),cv))
# Get Centre
center_x = (cv[0,0] + cv[1,0])/2
center_y = (cv[0,1] + cv[1,1])/2
cv[-2,0] = center_x
cv[-2,1] = center_y
if event.button == 3:
if lowest_dist < 1:
cv = np.delete(cv, (lowest_index), axis=0)
def onpress(self,event):
self.press=True
def onmove(self,event):
if self.press:
self.move=True
# Find the closest point to the click
global cv
lowest_dist = 1000
lowest_index = -1
for i in range(0, cv.shape[0]):
dist = np.sqrt((cv[i,0]-event.xdata)**2 + (cv[i,1]-event.ydata)**2)
if dist < lowest_dist:
lowest_dist = dist
lowest_index = i
if lowest_dist < 2:
cv[lowest_index, 0] = event.xdata
cv[lowest_index, 1] = event.ydata
print(cv)
def onrelease(self,event):
if self.press and not self.move:
self.onclick(event)
else:
global released
released = True
self.press=False; self.move=False
import rospy
from curtain_maker.srv import ProcessPoints, ProcessPointsRequest, ProcessPointsResponse
from geometry_msgs.msg import PoseArray, Pose, Point
"""
I need a brute force solver to figure out what the ideal beta values are given a set of points
that ensures curve doesnt exceed limit and acceleration is limited
Another thing! Also even if it does exceed. Does the final result actually hit the point or at least minimizes to that
Also, velocity should stay positive or continuous
# Check. Going down being sharper is better
# Going up being smoother is better?
# SOLVE! Put less points between distances that are short!!
# SOLVE! Actually, optimize for beta so that i keep within velocity bound and acceleration is minimized.
# and non negative
"""
def convert_forward(pts):
poses = []
for i in range(0, pts.shape[0]):
pose = Pose()
pose.position.x = pts[i, 0]
pose.position.z = pts[i, 1]
poses.append(pose)
poseArray = PoseArray()
poseArray.poses = poses
return poseArray
def convert_back(pts, rays):
np_pts = np.zeros((len(pts.poses),2))
np_rays = np.zeros((len(rays.poses), 3))
angles = []
velos = []
accels = []
for i in range(0, len(pts.poses)):
np_pts[i,0] = pts.poses[i].position.x
np_pts[i,1] = pts.poses[i].position.z
angles.append(pts.poses[i].orientation.w)
velos.append(pts.poses[i].orientation.x)
accels.append(pts.poses[i].orientation.y)
for i in range(0, len(rays.poses)):
np_rays[i,0] = rays.poses[i].position.x
np_rays[i,1] = rays.poses[i].position.y
np_rays[i,2] = rays.poses[i].position.z
return np_pts, angles, velos, accels, np_rays
def process_points_client(camera_name, points):
points = convert_forward(points)
rospy.wait_for_service('/carla_lcfuse/process_points')
try:
process_points = rospy.ServiceProxy('/carla_lcfuse/process_points', ProcessPoints)
output_msg = process_points(camera_name, points)
output_pts, angles, velos, accels, np_rays = convert_back(output_msg.output_pts, output_msg.laser_rays)
return output_pts, np.array(angles), np.array(velos), np.array(accels), np_rays
except rospy.ServiceException, e:
print "Service call failed: %s"%e
import curses, time
from scipy import signal
import scipy.signal
def peak_finding(x, N=5, threshold=0.12):
array = []
# Find change in direction
for i in range(N, x.shape[0]-N):
left_pt = x[i-N]
mid_pt = x[i]
right_pt = x[i+N]
left_dist = mid_pt - left_pt
right_dist = right_pt - mid_pt
if(np.sign(left_dist) != np.sign(right_dist)):
dist = min(np.abs(left_dist), np.abs(right_dist))
if dist > threshold:
array.append([i, dist])
# Consolidate
clusters = []
curr_cluster = []
for i in range(0, len(array)):
if len(curr_cluster) == 0:
curr_cluster.append(array[i])
elif abs(curr_cluster[-1][0] - array[i][0]) < N:
curr_cluster.append(array[i])
else:
# Choose the max in the cluster
maxsize = 0
best_item = None
for item in curr_cluster:
if item[1] > maxsize:
maxsize = item[1]
best_item = item
clusters.append(best_item)
curr_cluster = []
if len(clusters):
return np.array(clusters)
else:
return np.zeros((2,2))
def optimize():
global cv
best_b = None
lowest_cost = 1000000000.
costs = []
for b in np.arange(1.5, 10, 0.5):
cv[:,2] = b
cvT = cv.T
spline = bspline.process(cvT[0],cvT[1],cvT[2],plt.gca(), 1, draw=False)
spline = np.flip(spline, 0)
output_pts, angles, velos, accels, rays = process_points_client("camera01", spline)
velos = velos[np.isfinite(velos)]
accels = accels[np.isfinite(accels)]
# Accels
N = 5
accels = running_mean(accels, N)
peaks = peak_finding(accels)
summed_peak = np.sum(peaks[:,1]**2)
avg_peak = np.mean(peaks[:,1])
maximum_peak = np.max(peaks[:,1])
max_velo = np.max(velos)
# print(peaks.shape)
# print(np.max(peaks[:,1]))
delt = 0.01
cost = (1-delt)*float(summed_peak) + delt*max_velo
print((b, cost))
costs.append(cost)
#cost += summed_peak
#print((b, maximum_peak))
# # Velocity
# max_velo = np.max(velos)
# if(max_velo > 135000): cost+=10000
#cost += np.max(np.abs(accels))
# CHeck cost
if cost < lowest_cost:
lowest_cost = cost
best_b = b
# **
#print("Velo Max: " + str(np.max(velos)))
#print("Accel Min: " + str(np.min(accels)) + " " + str(np.min(accels)))
print(b)
print(best_b)
cv[:,2] = best_b
# plt.figure(2)
# plt.plot(costs)
# plt.pause(5)
def running_mean(x, N):
cumsum = np.cumsum(np.insert(x, 0, 0))
return (cumsum[N:] - cumsum[:-N]) / float(N)
if __name__ == "__main__":
# # Test
# pts = np.ones((5,2))*5
# for i in range(0, pts.shape[0]):
# pts[i, 0] = i
# process_points_client("camera01", pts)
# stop
import matplotlib.pyplot as plt
import matplotlib.lines as mlines
first_run = True
click = None
bspline = BSpline()
def test(closed):
global click
global first_run
global cv
global released
plt.figure(1)
plt.cla()
if(first_run):
first_run = False
click = Click(plt.gca(), button=1)
cvT = cv.T
#ax2 = plt.gca()
spline = bspline.process(cvT[0],cvT[1],cvT[2],plt.gca())
spline = np.flip(spline, 0)
output_pts, angles, velos, accels, rays = process_points_client("camera01", spline)
if released:
print("Optimize")
optimize()
released = False
# # Plot Rays
# for i in range(0, rays.shape[0], 1):
# x1 = 0
# y1 = 0
# x2 = rays[i,0]*10
# y2 = rays[i,2]*10
# l = mlines.Line2D([x1,x2], [y1,y2])
# l.set_alpha(.5)
# plt.gca().add_line(l)
N = 5
velos = velos[np.isfinite(velos)]
accels = accels[np.isfinite(accels)]
accels = running_mean(accels, N)
peaks = peak_finding(accels)
summed_peak = str(np.sum(peaks[:,1]**2))
max_velo = np.max(velos)
print("Peak Accel: " + str(summed_peak))
print("Velos Max: " + str(max_velo))
delt = 0.01
print("Cost: " + str((1-delt)*float(summed_peak) + delt*max_velo))
print(peaks)
# **
plt.scatter(output_pts[:, 0], output_pts[:, 1], s=1.0, c='r')
# Visualize
# plt.plot(x,y,'k-',label='Curve')
plt.minorticks_on()
plt.legend()
plt.xlabel('x')
plt.ylabel('y')
plt.xlim(-7, 7)
plt.ylim(0, 10)
#plt.gca().set_aspect('equal', adjustable='box-forced')
#plt.figure(2)
# # **
# plt.pause(0.001)
# return
plt.figure(2)
plt.ylim(5000, 20000)
plt.plot(velos)
plt.figure(3)
plt.plot(accels)
plt.figure(2)
plt.pause(0.001)
plt.figure(3)
plt.pause(0.001)
#while 1: plt.pause(0.001)
plt.figure(2)
plt.cla()
plt.figure(3)
plt.cla()
while 1:
test(False)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment