Created
April 21, 2022 16:02
-
-
Save Fabien-B/e9227ff99290114d4d3a986cb4fe48ac to your computer and use it in GitHub Desktop.
Simple robot simulator
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #!/usr/bin/python3 | |
| import time | |
| from math import cos, sin, atan2, sqrt, pi | |
| import threading | |
| SIMU_PERIOD = 0.01 | |
| ACCEL_MAX = 1000 | |
| ACCEL_ANG_MAX = 100 | |
| def normalize_angle(angle): | |
| a = angle | |
| while a >= pi: | |
| a -= 2*pi | |
| while a < -pi: | |
| a += 2*pi | |
| return a | |
| class Simu(threading.Thread): | |
| def __init__(self, pos_init): | |
| threading.Thread.__init__(self) | |
| self.pos = pos_init | |
| self.speed = (0, 0) | |
| self.target_speed = (0, 0) | |
| self.request_stop = False | |
| def set_speed(self, speed): | |
| self.target_speed = speed | |
| # self.speed = speed | |
| def update_speed(self, dt): | |
| """ | |
| limit speed change according to ACCEL_MAX and ACCEL_ANG_MAX | |
| """ | |
| vx, vtheta = self.speed | |
| vx_tg, vtheta_tg = self.target_speed | |
| if abs((vx_tg-vx)/dt) < ACCEL_MAX: | |
| vx = vx_tg | |
| else: | |
| if vx_tg-vx > 0: | |
| vx += ACCEL_MAX*dt | |
| else: | |
| vx -= ACCEL_MAX*dt | |
| if abs((vtheta_tg-vtheta)/dt) < ACCEL_ANG_MAX: | |
| vtheta = vtheta_tg | |
| else: | |
| if vtheta_tg-vtheta > 0: | |
| vtheta += ACCEL_ANG_MAX*dt | |
| else: | |
| vtheta -= ACCEL_ANG_MAX*dt | |
| self.speed = (vx, vtheta) | |
| def update_pos(self, dt): | |
| """ | |
| Update robot position | |
| """ | |
| # destructuring position and speed tuples | |
| x, y, theta = self.pos | |
| vxr, vtheta = self.speed | |
| # take the average angle of the robot during the last period | |
| theta_avr = theta + vtheta*dt/2 | |
| # convert speed from robot reference system to table reference system | |
| vx0 = cos(theta_avr) * vxr | |
| vy0 = sin(theta_avr) * vxr | |
| # update position | |
| x += vx0*dt | |
| y += vy0*dt | |
| theta += vtheta*dt | |
| self.pos = (x, y, normalize_angle(theta)) | |
| def stop(self): | |
| self.request_stop = True | |
| self.join() | |
| def run(self): | |
| prev = time.time() | |
| time.sleep(SIMU_PERIOD) | |
| while not self.request_stop: | |
| now = time.time() | |
| dt = now - prev | |
| self.update_speed(dt) | |
| self.update_pos(dt) | |
| prev = now | |
| time.sleep(SIMU_PERIOD) | |
| if __name__ == "__main__": | |
| s = Simu((0,0, 0)) | |
| s.start() | |
| s.set_speed((10, 0)) | |
| time.sleep(1) | |
| print(s.pos) | |
| s.set_speed((5, 0.2)) | |
| time.sleep(1) | |
| print(s.pos) | |
| s.stop() | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment