def test_Set2(self):
vibes.beginDrawing ();
vibes.newFigure("set-sep");
f = Function("x", "y", "x^2+(y+1)^2 - 4")
set = Set(f,CmpOp.LEQ, 0.2)
to_vibes = ToVibes(5)
set.visit(to_vibes);
vibes.endDrawing();
def test_Set2(self):
vibes.beginDrawing()
vibes.newFigure("set-sep")
f = Function("x", "y", "x^2+(y+1)^2 - 4")
set = Set(f, CmpOp.LEQ, 0.2)
to_vibes = ToVibes(5)
set.visit(to_vibes)
vibes.endDrawing()
def test_Paving2(self):
P = IntervalVector(2, [-4, 4])
A = Paving(P,YES);
# B = Paving(P,BoolInterval(YES));
f = Function("x", "y", "x^2 + y^2 - 4")
# pdc = PdcFwdBwd(f, CmpOp.LEQ)
pdc = myPdc()
A.Sivia(pdc,op_And,0.3);
vibes.beginDrawing()
vibes.newFigure('Test')
A.visit(ToVibes(10))
vibes.endDrawing()
def test_Paving2(self):
P = IntervalVector(2, [-4, 4])
A = Paving(P, YES)
# B = Paving(P,BoolInterval(YES));
f = Function("x", "y", "x^2 + y^2 - 4")
# pdc = PdcFwdBwd(f, CmpOp.LEQ)
pdc = myPdc()
A.Sivia(pdc, op_And, 0.3)
vibes.beginDrawing()
vibes.newFigure('Test')
A.visit(ToVibes(10))
vibes.endDrawing()
def test_Set3(self):
fx = Function("x", "y", "(x^2-[64,81],y^2-[64,81])")
sepx = SepFwdBwd(fx, CmpOp.LEQ)
set = Set(2)
sepx.contract(set, 8.0)
vibes.beginDrawing()
vibes.newFigure("set-example")
to_vibes = ToVibes(5)
set.visit(to_vibes)
vibes.endDrawing()
set.save("set-example")
def test_Set3(self):
fx = Function("x","y","(x^2-[64,81],y^2-[64,81])");
sepx = SepFwdBwd(fx,CmpOp.LEQ);
set = Set(2);
sepx.contract(set,8.0);
vibes.beginDrawing ();
vibes.newFigure("set-example");
to_vibes = ToVibes(5)
set.visit(to_vibes);
vibes.endDrawing();
set.save("set-example");
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()
