def simulationControl(aPend, Q0, QTarget, Kp, Kpd):
from scipy.integrate import solve_ivp
from time import time
Qp = Q0.copy()
Qpd = Q0.copy()
# Init pygame
pygame.init()
thisClock = pygClock()
screen = pygame.display.set_mode((600, 600))
scale = 100
offset = np.array([300, 300]).astype(np.int_)
# Function for integration -> Proportional
fIntP = lambda t, Q: aPend.getQd(Q.reshape((2, 1)),
u=np.dot(Kp, Q - QTarget))
fIntPD = lambda t, Q: aPend.getQd(Q.reshape((2, 1)),
u=np.dot(Kpd, Q - QTarget))
tStart = time()
thisClock.tick()
t = 0. # current time
while time() < tStart + 20.: # Simulate twenty seconds
screen.fill(
(255, 255,
255)) # Make screen white again -> otherwise all are displayed
pygDraw.line(
screen, (125, 125, 125), (300, 300),
(300 - int(1.5 * aPend.l * scale * np.sin(QTarget[0, 0])),
300 - int(1.5 * aPend.l * scale * np.cos(QTarget[0, 0]))))
# Plot current
# Plot P -> red
aPend.drawPyGame(screen, (255, 0, 0), scale, offset, q=Qp[:1, [0]])
# Plot PD -> black
aPend.drawPyGame(screen, (0, 0, 0), scale, offset, q=Qpd[:1, [0]])
dt = float(thisClock.tick(
30)) / 1000. # Let time pass, at least as much to have 30fps max
# Refresh
pygame.display.flip()
# Compute new pos
# SP
sol = solve_ivp(fIntP, [t, t + dt], Qp.squeeze(), vectorized=True)
Qp = sol['y'][:, [-1]]
sol = solve_ivp(fIntPD, [t, t + dt], Qpd.squeeze(), vectorized=True)
Qpd = sol['y'][:, [-1]]
# Update time
t += dt
pygame.display.quit()
return None
def simulationNoControlComp(aPend: pendule, qInit):
"""Simulate no control pendulum and show the difference between solve_ivp and forward euler"""
from scipy.integrate import solve_ivp
from time import time
QForward = qInit.copy()
QSolve = qInit.copy()
# Init pygame
pygame.init()
thisClock = pygClock()
screen = pygame.display.set_mode((600, 600))
scale = 100
offset = np.array([300, 300]).astype(np.int_)
# Function for integration
fInt = lambda t, Q: aPend.getQd(Q.reshape((2, 1)), u=np.zeros((1, 1)))
tStart = time()
thisClock.tick()
t = 0. # current time
while time() < tStart + 20.: #Simulate twenty seconds
screen.fill(
(255, 255,
255)) #Make screen white again -> otherwise all are displayed
# Plot current
# Plot euler -> red
aPend.drawPyGame(screen, (255, 0, 0),
scale,
offset,
q=QForward[:1, [0]])
# Plot solve -> black
aPend.drawPyGame(screen, (0, 0, 0), scale, offset, q=QSolve[:1, [0]])
dt = float(thisClock.tick(
30)) / 1000. # Let time pass, at least as much to have 30fps max
# Refresh
pygame.display.flip()
# Compute new pos
# Euler
QForward += dt * aPend.getQd(QForward, np.zeros((1, 1)))
#Solve
sol = solve_ivp(fInt, [t, t + dt], QSolve.squeeze(), vectorized=True)
QSolve = sol['y'][:, [-1]]
# Update time
t += dt
pygame.display.quit()
return None
def simulationControl(env):
from time import time
screen_size = 800
# Init pygame
pygame.init()
thisClock = pygClock()
screen = pygame.display.set_mode((screen_size, screen_size))
scale = 100
offset = np.array([300, 300]).astype(np.int_)
image = pygame.image.load("tortoise.jpg").convert()
image = pygame.transform.scale(image, (60, 45))
background = pygame.image.load("beach.png").convert()
# background = pygame.transform.scale(background, (800, 800))
tStart = time()
thisClock.tick()
seed(a=None, version=2)
for r in env.robots:
seed(a=None, version=2)
radius = randint(15, 50)
rot_speed = randint(1, 3)
const = randint(1,3)
x = randint(10, screen_size - 10)
y = randint(10, screen_size - 10)
env.goToPos(r.nom, x, y)
r.const = const
screen.blit(image, (int(r.pos[-1].x), int(r.pos[-1].y)))
t = 0. # current time
env.goToPos('rob1', screen_size/2, screen_size/2)
while time() < tStart + 20.: # Simulate twenty seconds
pygame.event.get()
screen.fill((255, 255, 255)) # Make screen white again -> otherwise all are displayed
screen.blit(background, (0, 0))
# Plot current
#aPend.drawPyGame(screen, (255, 255, 0))
#screen.blit(image, (int(rob2.pos[-1].x) , int(rob2.pos[-1].y) ))
vr = 1 * sin(1 * t)
vt = 60
dt = float(thisClock.tick(30)) / 1000. # Let time pass, at least as much to have 30fps max
for r in env.robots:
#env.trajCirc(r.nom, radius*r.const, rot_speed, step=0.01, duree=1)
#env.goToPos(r.nom,radius*r.const,const*r.const*radius)
#env.controleur(r.nom, x_des=1+screen_size/2, y_des=1+screen_size/2,kvit=0.1,kangle=.1)
screen.blit(image, (int(r.pos[-1].x), int(r.pos[-1].y)))
r.simulMCI(dt, vt, vr)
#screen.blit(image, (int(rob1.pos[-1].x)+screen_size/2, int(rob1.pos[-1].y)+screen_size/2))
# for r in env.robots:
# env.trajCirc(r.nom, radius, rot_speed, step=0.01, duree=1)
# Refresh
pygame.display.flip()
# vr = 1 * sin(1 * t)
# vt = 60
# env.simulMCI(dt, vt, vr)
# Update time
t += dt
pygame.display.quit()
return None
