def regulate(self,t):
#Determine next point
delta = 2*self.numero*math.pi/self.totalRobots
nextX, nextV = traj(t,delta)
vibes.drawCircle(nextX[0],nextX[1], 0.005, 'r')
self.setObjectifs(nextX, nextV)
#Calculate the new command
u = self.controlCommande()
return uTOpwm(u)
def shapeGen(): x, y = yield (x,y) = yield vibes.drawArrow([x-1, y-1], [x+1, y+1], 0.6, 'k[k]') (x,y) = yield vibes.drawAUV(x,y,45,2, 'k[y]') (x,y) = yield vibes.drawBox(x-1,x+1,y-2,y+2, "[b]") (x,y) = yield vibes.drawBoxesUnion([[x-1,x,y-2,y], [x-0.5,x+1,y-0.5,y+2]], "[r]") (x,y) = yield vibes.drawCircle(x,y,1, "g[m]") (x,y) = yield vibes.drawEllipse(x,y,2,1,45, color="r[darkCyan]") (x,y) = yield vibes.drawLine([[x-0.5, y-1], [x+1, y+0.5]], "k") (x,y) = yield vibes.drawPie( (x-2, y-2), (1,2.5), [20,70] , "y[cyan]") (x,y) = yield vibes.drawPolygon([[x-1,y-1], [x, y+1], [ x+1, y-1]], "k[orange]") (x,y) = yield vibes.drawRing(x,y, 1,2, '[red]') # (x,y) = yield vibes.drawPie( (x, y), (1,2), [0,360] , "y[cyan]") (x,y) = yield vibes.drawVehicle( x, y, 20, 1, "[darkBlue]") (x,y) = yield vibes.drawPoint( x,y, 1, "[k]") (x,y) = yield vibes.drawText( x,y, "Text", 1)
def shapeGen():
x, y = yield
(x, y) = yield vibes.drawArrow([x - 1, y - 1], [x + 1, y + 1], 0.6, 'k[k]')
(x, y) = yield vibes.drawAUV(x, y, 45, 2, 'k[y]')
(x, y) = yield vibes.drawBox(x - 1, x + 1, y - 2, y + 2, "[b]")
(x, y) = yield vibes.drawBoxesUnion(
[[x - 1, x, y - 2, y], [x - 0.5, x + 1, y - 0.5, y + 2]], "[r]")
(x, y) = yield vibes.drawCircle(x, y, 1, "g[m]")
(x, y) = yield vibes.drawEllipse(x, y, 2, 1, 45, color="r[darkCyan]")
(x, y) = yield vibes.drawLine([[x - 0.5, y - 1], [x + 1, y + 0.5]], "k")
(x, y) = yield vibes.drawPie((x - 2, y - 2), (1, 2.5), [20, 70], "y[cyan]")
(x,
y) = yield vibes.drawPolygon([[x - 1, y - 1], [x, y + 1], [x + 1, y - 1]],
"k[orange]")
(x, y) = yield vibes.drawRing(x, y, 1, 2, '[red]')
# (x,y) = yield vibes.drawPie( (x, y), (1,2), [0,360] , "y[cyan]")
(x, y) = yield vibes.drawVehicle(x, y, 20, 1, "[darkBlue]")
(x, y) = yield vibes.drawPoint(x, y, 1, "[k]")
(x, y) = yield vibes.drawText(x, y, "Text", 1)
m[r][1] = Interval(position[i][r][2]).inflate(PositionIncertitude) #y
t = m[i][0][0]
#SIVIA test for Robots secure area whith incertitude
pdcRobots = IncertitudeRobots(m,r**2)
# SIVIA test for Trail
sizeKernelErosion = enemySpeed*(t-t_old)/(1000*echellePixel)
print(sizeKernelErosion)
imgTrail = computeTrail(imgOut, enemySpeed)
imgIntegralTrail = cv2.integral(imgTrail)
pdcTrail = ImageToBoxes(imgIntegralTrail,i0,j0,echellePixel)
vibes.clearFigure('Robotique')
boatBoxesNP = SIVIA(X0, pdcGascogne, pdcTrail, pdcRobots, epsilon)
#Draw of the robots:
for m_ in m:
vibes.drawCircle(m_[0].mid(), m_[1].mid(), 0.2, '[k]')
vibes.drawArrow([-15, -15], [-15, -10], 1, 'w[w]')
vibes.drawArrow([-15, -15], [-10, -15], 1, 'w[w]')
#Creation of the output image:
imgT=np.zeros((j_max, i_max),dtype="uint8")
imgOut=cv2.fillPoly(imgT,np.array( boatBoxesNP),(1,1,1))
t_old = t
vibes.endDrawing()
# heading
theta_noise = dtheta * (2 * np.random.rand() - 1)
theta_measured = X[2, 0] + theta_noise
theta = Interval(theta_measured - dtheta, theta_measured + dtheta)
u = np.array([[0, 0.3]]).T # command
X += dt * f(X, u) # Euler's scheme
"""localization step"""
if t > tm * i:
if i == 0: # first SIVIA
fPred = Function(
"x", "y", "({0}-x)^2 + ({1}-y)^2".format(X[0, 0], X[1, 0]))
ctc = CtcFwdBwd(fPred, Interval(0, 1))
else:
# translation of the previous contractor because the robot has moved
fPred = Function(
'x', 'y',
'(x - {0}*{1}*cos({2}), y - {0}*{1}*sin({2}))'.format(
tm, v, theta))
ctc = CtcInverse(ctc, fPred)
pySIVIA(P, ctc, 0.5)
vibes.drawCircle(*X[:2, 0].flatten().tolist(), 0.4,
"[red]") # draw robot
i += 1
time.sleep(dt)
vibes.endDrawing()
from vibes import vibes
print(vibes.channel, vibes.current_fig)
vibes.beginDrawing()
vibes.newFigure("test")
vibes.drawBox(0,1,4,6,color='[#FF12FFA0]', figure='test')
vibes.drawText(12, 12, 'My Text', 0.1, 'b[r]')
vibes.axisAuto()
vibes.drawBox(0.5,2,4,7,color='[#00FFFF66]', figure='test')
vibes.drawBox(-1,0,-4,-6,color='r', figure='test')
vibes.drawLine([[0,0], [4,4]])
vibes.drawEllipse(3, 5, 1, 3, 45)
vibes.drawEllipse(5, 5, 3, 3, 0, angles=[30,60], color='g')
vibes.drawCircle(0, 0, 5)
vibes.drawAUV(0,0, 2, 3, color='r[yellow]')
vibes.drawPie([0,0], [3,4], [45, 90])
vibes.newGroup("Pie", figure="test", format='[cyan]')
vibes.drawPie([0,0], [5,9], [-120, -40], group="Pie")
# vibes.drawPie([0,0], [5,9], [-120, -40], "[b]")
vibes.drawPie([5,2], [1,2], [160, 220]) #, 'g[y]')
# vibes.clearGroup("Pie", figure="test")
vibes.drawPie([0,0], [1,2], [160, 220]) #, 'g[y]')
vibes.beginDrawing()
for t in T:
print(t)
u = np.array([[0, 0]]).T # command
X += dt * f(X, u) # Euler's scheme
if t > tm * (i + 1): # SIVIA
d = getDistanceFromLandmark(X)
fCorr = pyibex.Function(
"x", "y", "({0}-x)^2 + ({1}-y)^2".format(a[0, 0], a[1, 0]))
if i == 0: # first SIVIA
ctc = pyibex.CtcFwdBwd(fCorr, sqr(d))
else:
fPred = pyibex.Function(
'x', 'y',
'(x - {0}*{1}*cos({2}), y - {0}*{1}*sin({2}))'.format(
tm, X[3, 0], X[2, 0]))
ctc_pred = pyibex.CtcInverse(ctc, fPred)
ctc_corr = pyibex.CtcFwdBwd(fCorr, sqr(d))
ctc = ctc_pred & ctc_corr
pyibex.pySIVIA(P, ctc, 0.1)
vibes.drawCircle(*X[:2, 0].flatten().tolist(), 0.4,
"[red]") # draw robot
vibes.drawCircle(*a[:2, 0].flatten().tolist(), 0.4,
"[black]") # draw landmark
i += 1
time.sleep(dt)
vibes.clearFigure()
vibes.drawLine([[-4.4,47.8],[-3.8,47.8],[-2.3,47.2],[-2,46.6],[-1.3,44.4],[-2.0,43.3],[-3.6,43.5],[-2.8,43.4]])
for t in frange(0,100,h):
time.sleep(0.1)
#vibes.clearFigure()
time1 = time.time()
for rob in listRobots:
#Draw circle of position
# print(rob.X[0])
# print(rob.X[1])
vibes.drawAUV(rob.X[0], rob.X[1], 0.2, rob.theta*180/math.pi, rob.couleur)
#Determine next point
delta = 2*rob.numero*math.pi/len(listRobots)
nextX, nextV = traj(t,delta)
vibes.drawCircle(nextX[0],nextX[1], 0.005, 'r')
# print(nextX)
# print(nextV)
rob.setObjectifs(nextX, nextV)
#Calculate the new command
rob.controlCommande()
#Move the robot
rob.subMove()
vibes.endDrawing()
u = np.array([[0, 0.3]]).T # command
X += dt * f(X, u) # Euler's scheme
"""localization step"""
if t > tm * i:
h = getDepth(X)
if i == 0: # first SIVIA
# generation of the contractor associated with the current sea depth measurement
ctc = getCtcBathy(bathy, h, dh)
else:
# translation of the previous contractor because the robot has moved
fPred = pyibex.Function(
'x', 'y',
'(x - {0}*{1}*cos({2}), y - {0}*{1}*sin({2}))'.format(
tm, v, theta))
ctc_pred = pyibex.CtcInverse(ctc, fPred)
# generation of the contractor associated with the current sea depth measurement
ctc_corr = getCtcBathy(bathy, h, dh)
# fusion
ctc = ctc_pred & ctc_corr
pyibex.pySIVIA(P, ctc, 30)
vibes.drawCircle(*X[:2, 0].flatten().tolist(), 25,
"red") # draw robot
i += 1
time.sleep(dt)
vibes.endDrawing()
from vibes import vibes
print(vibes.channel, vibes.current_fig)
vibes.beginDrawing()
vibes.newFigure("test")
vibes.drawBox(0, 1, 4, 6, color='[#FF12FFA0]', figure='test')
vibes.drawText(12, 12, 'My Text', 0.1, 'b[r]')
vibes.axisAuto()
vibes.drawBox(0.5, 2, 4, 7, color='[#00FFFF66]', figure='test')
vibes.drawBox(-1, 0, -4, -6, color='r', figure='test')
vibes.drawLine([[0, 0], [4, 4]])
vibes.drawEllipse(3, 5, 1, 3, 45)
vibes.drawEllipse(5, 5, 3, 3, 0, angles=[30, 60], color='g')
vibes.drawCircle(0, 0, 5)
vibes.drawAUV(0, 0, 2, 3, color='r[yellow]')
vibes.drawPie([0, 0], [3, 4], [45, 90])
vibes.newGroup("Pie", figure="test", format='[cyan]')
vibes.drawPie([0, 0], [5, 9], [-120, -40], group="Pie")
# vibes.drawPie([0,0], [5,9], [-120, -40], "[b]")
vibes.drawPie([5, 2], [1, 2], [160, 220]) #, 'g[y]')
# vibes.clearGroup("Pie", figure="test")
vibes.drawPie([0, 0], [1, 2], [160, 220]) #, 'g[y]')
# exit()
from vibes import vibes
import time
from SaveLoadData import *
if __name__ == '__main__':
A=[1,1]
B=[0,-1]
C=[-1,0]
vibes.beginDrawing()
vibes.newFigure('TestCase1')
vibes.setFigureProperties({'x': 0, 'y': 0,'width': 500, 'height': 500})
vibes.axisLimits(-25, 25, -25, 25)
t,dt,Field,inc,dif,x,y,xmp,ymp,xm,ym = loadData("rot1.pckl")
t,dt,Field,inc,dif,x2,y2,xmp,ymp,xm,ym = loadData("rot2.pckl")
time.sleep(15)
for i in range(len(t)):
vibes.drawCircle(x[i], y[i], 1, 'black[green]')
vibes.drawCircle(x2[i], y2[i], 1, 'black[red]')
vibes.drawCircle(xm, ym, 0.4, 'black[blue]')
vibes.drawArrow([20, 15], [15, 15], 1, 'black[black]')
vibes.drawArrow([20, 10], [15, 10], 1, 'black[black]')
vibes.drawArrow([20, 5], [15, 5], 1, 'black[black]')
vibes.drawArrow([20, 0], [15, 0], 1, 'black[black]')
vibes.drawArrow([20, -5], [15, -5], 1, 'black[black]')
time.sleep(0.001)
vibes.clearFigure()
vibes.endDrawing()
from vibes import vibes
from pyIbex import PixelMap2D, CtcPixelMap, IntervalVector, Interval
from scipy import misc
import numpy as np
"""
Example : image contractor
"""
# Generate the image using vibes
# Only use for drawing
vibes.beginDrawing()
vibes.newFigure('Map')
vibes.setFigureProperties({'x': 0, 'y': 0, 'width': 500, 'height': 500})
vibes.drawCircle(4, 4, 3, '[k]')
vibes.drawLine([[-3, -1], [7, -1]], '[k]')
vibes.drawLine([[7, -1], [7, 1]], '[k]')
vibes.axisLimits(-4, 8, -4, 8)
# Read the image and extract the last layer
img = misc.imread('./img.png')[:, :, 3]
# normalize in order to have a binary image.
img = img / np.max(img)
if np.max(img) != 1:
print("Error, img must be a binary image")
# convert img into a unsigned int64 image type
img = img.astype(np.uint64)
# vertical flip to inverse the y axis
img = np.flipud(img)
# PixelMap2D constructor takes :
sep = SepQInterProjF(seps)
sep.q = 0
# init drawing area
vibes.beginDrawing()
vibes.newFigure('Result')
vibes.setFigureProperties({'x': 0, 'y': 0, 'width': 1000, 'height': 1000})
#configure pySIVIA output
params = {'color_in': '#888888[#444444]', 'color_out':
'#888888[#DDDDDD]', 'color_maybe': '#888888[w]', 'use_patch' : True}
# create the initial box X0 = [-10, 10] x [-10, 10]
X0 = IntervalVector([[-12, 11], [-6, 17]]) # '#888888[#DDDDDD]'
# run SIVIA
(res_in, res_out, res_y) = pySIVIA(X0, sep, 0.1, **params)
vibes.drawAUV(robot[0], robot[1], 1, np.rad2deg(0.3))
for (x, y), d in zip(landmarks, dist):
vibes.drawCircle(x,y, 0.1, "[k]")
vibes.drawCircle(x,y, d.lb(), "k")
vibes.drawCircle(x,y, d.ub(), "k")
#equalize axis lenght
vibes.axisEqual()
vibes.endDrawing()
# init drawing area
vibes.beginDrawing()
vibes.newFigure('Result')
vibes.setFigureProperties({'x': 0, 'y': 0, 'width': 1000, 'height': 1000})
#configure pySIVIA output
params = {
'color_in': '#888888[#444444]',
'color_out': '#888888[#DDDDDD]',
'color_maybe': '#888888[w]',
'use_patch': True
}
# create the initial box X0 = [-10, 10] x [-10, 10]
X0 = IntervalVector([[-12, 11], [-6, 17]]) # '#888888[#DDDDDD]'
# run SIVIA
(res_in, res_out, res_y) = pySIVIA(X0, sep, 0.1, **params)
vibes.drawAUV(robot[0], robot[1], 1, np.rad2deg(0.3))
for (x, y), d in zip(landmarks, dist):
vibes.drawCircle(x, y, 0.1, "[k]")
vibes.drawCircle(x, y, d.lb(), "k")
vibes.drawCircle(x, y, d.ub(), "k")
#equalize axis lenght
vibes.axisEqual()
vibes.endDrawing()
from vibes import vibes
from pyIbex import PixelMap2D, CtcPixelMap, IntervalVector, Interval
from scipy import misc
import numpy as np
"""
Example : image contractor
"""
# Generate the image using vibes
# Only use for drawing
vibes.beginDrawing()
vibes.newFigure('Map')
vibes.setFigureProperties({'x': 0, 'y': 0, 'width': 500, 'height': 500})
vibes.drawCircle(4, 4, 3, '[k]')
vibes.drawLine([[-3, -1], [7, -1]], '[k]')
vibes.drawLine([[7, -1], [7, 1]], '[k]')
vibes.axisLimits(-4,8,-4,8)
# Read the image and extract the last layer
img = misc.imread('./img.png')[:,:,3]
# normalize in order to have a binary image.
img = img/np.max(img)
if np.max(img) != 1:
print("Error, img must be a binary image")
# convert img into a unsigned int64 image type
img = img.astype(np.uint64)
# vertical flip to inverse the y axis
img = np.flipud(img)
