From 2aed89a3438c3231fba725c5336d2062eecd1637 Mon Sep 17 00:00:00 2001 From: shaoanlu Date: Wed, 10 Apr 2024 08:33:21 +0900 Subject: [PATCH 1/3] refactor --- README.md | 20 +- .../demo_safety_filter_off.gif | Bin .../demo_safety_filter_on.gif | Bin .../__pycache__/__init__.cpython-36.pyc | Bin 0 -> 158 bytes .../__pycache__/i_controller.cpython-36.pyc | Bin 0 -> 814 bytes controllers/__pycache__/robot.cpython-36.pyc | Bin 0 -> 6018 bytes .../__pycache__/robot_cbf.cpython-36.pyc | Bin 0 -> 6327 bytes controllers/i_controller.py | 14 + robot.py => controllers/robot_cbf.py | 33 +- demo.py | 26 +- demo_codegen_osqp.py | 282 ------------------ models/__pycache__/__init__.cpython-36.pyc | Bin 0 -> 153 bytes models/__pycache__/i_model.cpython-36.pyc | Bin 0 -> 635 bytes models/__pycache__/models.cpython-36.pyc | Bin 0 -> 1743 bytes models/__pycache__/robot.cpython-36.pyc | Bin 0 -> 1797 bytes .../__pycache__/robot_dynamics.cpython-36.pyc | Bin 0 -> 1806 bytes models/i_model.py | 9 + models.py => models/robot_dynamics.py | 3 +- tests/__init__.py | 0 tests/__pycache__/__init__.cpython-36.pyc | Bin 0 -> 152 bytes tests/__pycache__/test_models.cpython-36.pyc | Bin 0 -> 2157 bytes tests/__pycache__/test_robot.cpython-36.pyc | Bin 0 -> 2757 bytes test_models.py => tests/test_models.py | 2 +- test_robot.py => tests/test_robot.py | 8 +- utils.py | 4 +- 25 files changed, 84 insertions(+), 317 deletions(-) rename demo_safety_filter_off.gif => assets/demo_safety_filter_off.gif (100%) rename demo_safety_filter_on.gif => assets/demo_safety_filter_on.gif (100%) create mode 100644 controllers/__pycache__/__init__.cpython-36.pyc create mode 100644 controllers/__pycache__/i_controller.cpython-36.pyc create mode 100644 controllers/__pycache__/robot.cpython-36.pyc create mode 100644 controllers/__pycache__/robot_cbf.cpython-36.pyc create mode 100644 controllers/i_controller.py rename robot.py => controllers/robot_cbf.py (88%) delete mode 100644 demo_codegen_osqp.py create mode 100644 models/__pycache__/__init__.cpython-36.pyc create mode 100644 models/__pycache__/i_model.cpython-36.pyc create mode 100644 models/__pycache__/models.cpython-36.pyc create mode 100644 models/__pycache__/robot.cpython-36.pyc create mode 100644 models/__pycache__/robot_dynamics.cpython-36.pyc create mode 100644 models/i_model.py rename models.py => models/robot_dynamics.py (93%) create mode 100644 tests/__init__.py create mode 100644 tests/__pycache__/__init__.cpython-36.pyc create mode 100644 tests/__pycache__/test_models.cpython-36.pyc create mode 100644 tests/__pycache__/test_robot.cpython-36.pyc rename test_models.py => tests/test_models.py (95%) rename test_robot.py => tests/test_robot.py (87%) diff --git a/README.md b/README.md index b53b882..12f3d98 100644 --- a/README.md +++ b/README.md @@ -4,12 +4,12 @@ This Python program demonstrates the use of control barrier functions (CBFs) for safe robot control in a simple 2D environment. The program simulates a controllable green robot navigating through a scenario with stationary and patrolling robots. CBFs are employed to avoid collisions between the green robot and other robots in the environment. When safety filter is activated, it modified the user command to prevent collision while keeping the modification minimal. -![](demo_safety_filter_off.gif) ![](demo_safety_filter_on.gif) +![](assets/demo_safety_filter_off.gif) ![](assets/demo_safety_filter_on.gif) (Left: w/o safety filter; Right: w/ safety filter) ## Dependencies -The program was develop[ed and tested in the following environment. +The program was developed and tested in the following environment. - Python 3.6+ - `torch==1.8.1+cpu` (optional) - `osqp==0.6.2.post8` @@ -21,8 +21,18 @@ The program was develop[ed and tested in the following environment. ### Execution Run `demo.py` to start the simulation. A simple version w/ code generation is provided as `demo_codegen_osqp.py`. ```bash -python demo.py # or python demo_codegen_osqp.py +python demo.py # a PyGame window will be spawn ``` + +Unit test can be executed through the following command +```bash +python -m unittest +# ........... +# ---------------------------------------------------------------------- +# Ran 11 tests in 0.000s +# OK +``` + ### Controls - `Arrow keys`: Control the green robot's movement. - `X`: Toggle CBF ON/OFF. @@ -34,8 +44,8 @@ python demo.py # or python demo_codegen_osqp.py ## Program Overview The program consists of the following classes and files: -- `Robot`: A class in `robot.py` representing a robot in the environment. It includes methods for controlling the robot and detecting collisions. -- `SimpleRobotDynamics`: A class in `models.py` defining the dynamics of the robot. It includes methods for calculating state transition, control barrier functions, and the derivatives of control barrier funcitons. +- `robot.py`: Implements the `Robot` class representing a robot in the environment. It includes methods for controlling the robot and detecting collisions. +- `models.py`: Implements the `SimpleRobotDynamics` class defining the dynamics of the robot. It includes methods for calculating state transition, control barrier functions, and the derivatives of control barrier funcitons. - `demo.py`: The entry point of the program that handles user input, robot movement, collision detection, and rendering. - `utils.py`: This file contains a helper function to draw the robots on the screen. diff --git a/demo_safety_filter_off.gif b/assets/demo_safety_filter_off.gif similarity index 100% rename from demo_safety_filter_off.gif rename to assets/demo_safety_filter_off.gif diff --git a/demo_safety_filter_on.gif b/assets/demo_safety_filter_on.gif similarity index 100% rename from demo_safety_filter_on.gif rename to assets/demo_safety_filter_on.gif diff --git a/controllers/__pycache__/__init__.cpython-36.pyc b/controllers/__pycache__/__init__.cpython-36.pyc new file mode 100644 index 0000000000000000000000000000000000000000..7ea5aae4eb97186f2901efacdb2a2ddb76d97e6a GIT binary patch literal 158 zcmXr!<>h*CMl_892p)q77+?f49Dul(1xTbY1T$zd`mJOr0tq9CU*67EF`>n&Ma40} zjy|O^F8Rr&xv6<2#WC)gB_5?oG0skI@qq#H#ffRDC6)1MnK>n?MKQ_wc_l^pIXNJ0 f@$s2?nI-Y@dIgoYIBatBQ%ZAE?Ld|k12F>t&6g?M literal 0 HcmV?d00001 diff --git a/controllers/__pycache__/i_controller.cpython-36.pyc b/controllers/__pycache__/i_controller.cpython-36.pyc new file mode 100644 index 0000000000000000000000000000000000000000..73ff3f1d09fb02c8092d198454ea5b41654100f5 GIT binary patch literal 814 zcma)4!EV$r5Vhm%Zn|tMq>2k5E^x`E2e=`GfCWk=w16ruQ6wvyaYQ61X~)i?J<|OQ zet>V`4|3(iFK}WeX}1*$5|(B%p3Ka9e)f~y-TA{GcZ$1Gh@axdQWU=+a}UUY0BJ=f z6Dh@^fB@7+fVv2h0K#JtPkvF2h-J5zt)0g#_m%Nq8pt_)wYCEvQ^tryf}nN@!8ZvC z!cCYc6T$?h8*w3%3B@yr`gjVGKz6$QWm9`>tTlL4dxJ%mn-!r>(fAhYoQ}{h{<1B} zTu2TC^~=?FrWiV73!G54cfUU@pQfLkL6;uCeBY+8o4l<|?Ol3U`giR*UA}s&KYi3L zD~vzWMQI5*&9_8cTI%cF!O2+{X`y0IqNrC>4gU8mLXJqOr71ujpvg0;6eGOxdOf znWj~-P&X=BZdP(UQ^|9yQn;q@EYCeqcy3><6gj)Cl=F{)TUjf!#Ejamh&p* z6+H8_;uY~4cuV~FLzAE2bHILtpXBool*&=Qz)#_OjGyMu;Cq}u%NOxI!Jp&L<2%P+ z;5NP|fsX;hiw_w;!(Rf1d0qmOUQG%S5Tt&0Q#)2I1nfxlTejVlThz#XgB&QpxM^}Go0!cR{*nI!HTps zxt6QtoX7%SqIS9{y(HHPn?5Ax;D1u@i98009DxE2&-;;s)doKIc%rvOxK%c=W-q8o zbm>SrhlW2=#2h|fzJKN7=0}kyqRrcvuXi?A!)mALwc=>=nje4A+1kAF{#EDZx)ZrI zFYY-tKZrfC3EW|v!liaEu^h*5`LW|11;&WbDjAbazk4Og@1@b;DlwiO`i?nCYwIi9 z$~Jg-nz*8-Tq&z5Q&_jm#0d?kFr$~`5`shrU!K@8FQ&7B|UC z$VP~ZYM<>hru0?uHvV8y+oUTD_GG}m%ze1ZmW395A4-L>9n~Ud(Oz_s$nY@VSo;|-41%RZD|ZJwX|b?*fBlf*a%ZQjC^8FB#K0) z1W?YTD!fPy&k-Q6BVHh26L^sT8PY_-lgyCf3YsI5s*){eIZbEjt1@~m&C>RYsp=2M zA2NWkX3k?iI3xL|0y@NJP4bAXmGucr4y<~%;k^t@B&$?A+1}#N%u{|%v8752phrg; zFNSs>lOS1Nt*iTNK%!M4V2#8*UR0vC+KJ%`;r3Ezkb(Yv>NHqs4R*LAolNGo6ELL_@aRw`Ad1aus zE}%t4%L=vN-^Ra&|JC6LtyMycj+R?;gofatNl`#;9rLN`nn}zLo$KGfx*-EVlKIfN z^@D34Y$W=J&PVIiw|e8w+C+XM-bh^=GiaJ4x4M5GrAv*epQ@HN@qbBB2}E!mL~ zlO0j~l1BVDRIob*#>v31%z(^O${7Z$_cMK~Z|r9_jX`#h8{`LtK@t53oI94d7i(n5 zscJ$SjDEJC>*xE0evzwO`S=v-TIC-CLPC)wVn5&^xm)T8fuuc8q8@UVFV;w*UFF-`nT^ z{hMF^1#}wJaQh9_zYF2+dJ~~;Ks$z#w%x687i!|R>@ClR z*J?@y;xr!}@+vtPisuBdff#u>yPvQiVV#P4xmzwJi%xZ9eNMxH4B+CuS}k&TgLe_6 zxmWXB9%ZaU&O3WE4xT6eS0R+YOO0~|xr1fZ@CC{?0}tD{AS zhei^`*+UtB`{a95FKg_l379r)$-sLvuGZNC4tq?wU%G?jtgqEnnQE1_MD2=1?Fo58 z5i%9EWWp;b)l7qUaTIf1;Wnf6EMsDCUW=bQbv|r7RrV z2Lm)fVg!BFzFybrVBNrqiKQ$LrsR<@$hNGUh4A$NL2(sqn}vZh!az(NyM# z&avFUJcLY|hc+DnP2E>z4RrVAO606O&A@q_sEkG%YxT&8CFpTjbun)s^&44mw*QE5Nv5ynd(NRgJVkDkANZqv%mDw-U`}*wz zDR5a$)TIjvt4@jc%iN{Fk7C5%_Pd{vTnMMB?7caZ>ui6pbSVfS@#tMtMzasCin2Y* zC}lb%S5+FYpDJJln^T@#n^?uzL~S@YGRh=grtb1sdKEo?BuweT4pAXRl6xojB&p;E z$Rf87?j9$V*U+12h>@rhA@L4@!>0p5bpC`gi_ZVE8*vTDW^d>7r;zyu+YuiS_OIE_ zpC@#1MkD@Ja@DAc^dc61SF%OCj@Be2V_&C<3c`Q0)0F6CdR#6(-Di)TNP7CZ2vpKl zi|l0C!b*abM26}F<=M&yROG|Peww61ZbkZO zj*7f6Qes#E`PLm>{J48vdhFE!nL4s!!12+xPHiw~bW-l0x`P`;7^J!P+i}3Y9)@jXRU+)v8@3w+_V8Seyo4@#qO#1b5HW4KO3Vzee!EIhef%qDzf7c^NWSV$G9}gEKwP0qq|v!Y#tY&tf;?6>WjD!D z?!`zI)0;E8l=v%#Mub2yOvH&CJ4ffk`P83G4}PSj_Xrec(?muSxH(k>2~<$vC@;|U z817g-$4PRIgGkj0s9tcK`yDq(dju&#Vjv|6yu_#lp&OIM$AXSyiG^II?TNTI*&=Rs zy%@nxkkKXjR){MKoqU;i6%FEL0(8n1X9&DRfC8Z))fMXmCjMjMtmsaY?m{9mD@(^E z3Y#w$^nzZ}OJ;h(K+s;mHG^&yt#4UJtT{omkeV5-PP5%hrDH@@A9WI*cpD%w<6awL yPnVk>&Z8w6wIjKALHAqY4Fcx~oF_ntXRR}_E~RXI7eP3+LJOioQ46E{5beJ>G8-8H literal 0 HcmV?d00001 diff --git a/controllers/__pycache__/robot_cbf.cpython-36.pyc b/controllers/__pycache__/robot_cbf.cpython-36.pyc new file mode 100644 index 0000000000000000000000000000000000000000..a987a210a821e7d4bb299d776940b54719cce77b GIT binary patch literal 6327 zcmbVQON<;x8Sd)GJbE79_1lh+@K{@kfk5!%yIEznVvLE13N(W05S2u>U_( zdQ3|hQ&I?L(@NOj)tQtA4Mm*jqmXVXRhdJDR3+WjMR=4a_LWl8)y@)Y(+hfH)&|c2 z(|ChlkJK?t+_kZklzay5dkGy!+xaP>2ZoTq6{r;?iAl}_BuCs{mwT~rCuTU-esp-U_51)8Dz>y#slUk7V3a5vD#2}Q?Do=KM~s==E|2r?=DJ2tI$}LZ5^NPVXP9FAmJNGa+w|OS~*7#sqIL|nvqUy z6y>cUi?t11p2QwlNgL^%^Jvl0vPdoXckplEe`!3z=v2{SqUDwvVIZOyN=8s=H}_Ok z&L_DK+-vV%xuZfwlK;TH_5RiO?j+_1?%f;Iw{-o(<*8sNTvB?Nv1%`GUc`>RixQE- zXsn=Kd&f7&wwx!pwZ|T6xv9u^D7w;n()P>;kS!KbE~7+$0009t*fs-8^CPlur4+Dt zbC@3%hSqj|#Tpey_NX)}k1FU#DBdW@`>{cOAXRIqjx{U}?O|zH9#*){4Q_s%hqpLP zaeQwG=g%fmfxVIgcb;$ z6jlkU49XRi_2qhZq1U!EelB}KqZh!(IPscrDAqcSu-*1L+*utsQ5b9rztd8kO+@(c zBl-)GB)0u6j~t0Z;XV*;J3~1^O5KiXxT&x)Leuc#< z=(fH7mr|mc=hfKHKT{vS)66D(v?ip;{%B~=`Ihs&UjK~K8{BjJNHKQo7ahOT?Ztpj z-y!}F9IuZNQbb(}XvhNxyEhGjV+<;mIh&sJJ;-7gLACEagjV6Bd?B4CBBYWbkxNaNM*g%ZTo;^rd zkg#4|f2UuoCMVp+#QNMd7cxLR5Y1-f@-^N^jOU{1cLe3YW6rxs20VEHZYVK{v32wM z?VC61RwUwHH_4@uAu(xGVx_#M&^&dqQ?FN>+Y^#zxxs|3C8eEfVmBmZWC?pmzD|f2 z>BoSECd}RIs86A5^#=svjcD8yZbrK8LgMnBq567p7(P14UGCFN<chk7+nH;XJO!sFc9-c_AoC$Cpf5@if$p|iiJDcS8Msg&DwEMxHrhgJ_B* z@4#J{;--Eoz>wiXDliW4O$$m>Tvl45GW;z4C5w2jZ*v{S(S~ZM&#s+owajqGFGbov!a> z0Mc_ho zl5FdCBWfTgiKX9HY>_pzCV3Swdu>#ZGqihcg%vpL3SQKXpt@Ir`f4p3HWUN+jUyW4%zCoLbq623?m>HykW}k z_pd3(zBD3BM;Z;79i>~;275-*tgq+kY<3jBL7mX(j8@>rL2ZM{qnz%&v3>vwc-}D7{6gFf`lt4#q5JCbL<9@GN3TPWF!f)R=o5PC5gd@PSpdja`RSIBrU-6igtJsH4Je}O0G=4 zf(A)<0O}YmU!>Y80u(Y+_T@V~J*AT#%bNtI&P3IemQ$)Rj}no)DVVs$VTUVavusw) zYA(IuAm|(^7;|QI-?XLrR&{>zXTmP32PEOD%_UZ+*X|C|RajBON1bFW-vUT-@t_Os zNVjxI1^;M4sg29W_wR~N^`qkx15_wl@F%wj_>&ePdyN2H(aEy}76=giS({7>7gLE` WqAMtMZ(4xhQS>q^o3who#(x2u&S3cf literal 0 HcmV?d00001 diff --git a/controllers/i_controller.py b/controllers/i_controller.py new file mode 100644 index 0000000..980d246 --- /dev/null +++ b/controllers/i_controller.py @@ -0,0 +1,14 @@ +from abc import abstractmethod + + +class ControllerInterface: + def __init__(self) -> None: + pass + + @abstractmethod + def control(self): + raise NotImplementedError + + @abstractmethod + def detect_collision(self): + raise NotImplementedError \ No newline at end of file diff --git a/robot.py b/controllers/robot_cbf.py similarity index 88% rename from robot.py rename to controllers/robot_cbf.py index 087ffc6..c6565cf 100644 --- a/robot.py +++ b/controllers/robot_cbf.py @@ -3,20 +3,22 @@ import osqp from typing import Optional -from models import SimpleRobotDynamics +from models.robot_dynamics import SimpleRobotDynamics +from controllers.i_controller import ControllerInterface +# IDs for detecting arrow keypress in PyGame K_LEFT = 1073741904 K_RIGHT = 1073741903 K_UP = 1073741906 K_DOWN = 1073741905 -class Robot: +class RobotCBF(ControllerInterface): def __init__( self, model: SimpleRobotDynamics, - color: tuple = (0, 255, 0), + color: tuple = (0, 0, 255), vel: float = 3, size: int = 30, ): @@ -25,8 +27,8 @@ def __init__( self.uy = 0 # actual control input self.nominal_ux = 0 # user control input self.nominal_uy = 0 # user control input + self.color = color # color of the robot in PyGame GUI self.vel = vel # robot velocity generated by use control input - self.color = color self.size = size self.is_collided = False self.prob = None @@ -80,11 +82,11 @@ def control( def _apply_nominal_control(self): self.ux, self.uy = self.nominal_ux, self.nominal_uy - def _update_positions(self, ux, uy): + def _update_positions(self, ux: float, uy: float): model_control = np.array([ux, uy]) self.model.forward(model_control) - def _update_nominal_control(self, key): + def _update_nominal_control(self, key: int): ux, uy = 0, 0 if key is not None: if key == K_LEFT: @@ -98,7 +100,13 @@ def _update_nominal_control(self, key): self.nominal_ux, self.nominal_uy = ux, uy def _apply_cbf_safe_control( - self, ux, uy, cbf_alpha, penalty_slack, force_direction_unchanged, collision_objects + self, + ux: float, + uy: float, + cbf_alpha: float, + penalty_slack: float, + force_direction_unchanged: bool, + collision_objects: list, ): """ Calculate the safe command by solveing the following optimization problem @@ -151,7 +159,7 @@ def _apply_cbf_safe_control( self.ux, self.uy = ux, uy - def _calculate_h_and_coeffs_dhdx(self, collision_objects): + def _calculate_h_and_coeffs_dhdx(self, collision_objects: list): h = [] # barrier values (here, remaining distance to each obstacle) coeffs_dhdx = [] # dhdt = dhdx * dxdt = dhdx * u for obj in collision_objects: @@ -167,7 +175,14 @@ def _calculate_h_and_coeffs_dhdx(self, collision_objects): return h, coeffs_dhdx def _define_QP_problem_data( - self, ux, uy, cbf_alpha, penalty_slack, h, coeffs_dhdx, force_direction_unchanged + self, + ux: float, + uy: float, + cbf_alpha: float, + penalty_slack: float, + h: np.ndarray, + coeffs_dhdx: np.ndarray, + force_direction_unchanged: bool, ): # P: shape (nx, nx) # q: shape (nx,) diff --git a/demo.py b/demo.py index b03d985..b47f073 100644 --- a/demo.py +++ b/demo.py @@ -2,8 +2,8 @@ import pygame import numpy as np -from models import SimpleRobotDynamics -from robot import Robot +from models.robot_dynamics import SimpleRobotDynamics +from controllers.robot_cbf import RobotCBF from utils import draw_robot @@ -16,10 +16,10 @@ def run(): pygame.key.set_repeat(10) # init robots - auto_robot = Robot(SimpleRobotDynamics(x0=np.array([50, 350])), (0, 255, 0), vel=3) - static_robot = Robot(SimpleRobotDynamics(x0=np.array([120, 200])), (0, 0, 255), vel=0) - patrol_robot1 = Robot(SimpleRobotDynamics(x0=np.array([230, 300])), (0, 0, 255), vel=1) - patrol_robot2 = Robot(SimpleRobotDynamics(x0=np.array([300, 70])), (0, 0, 255), vel=1) + ego_robot = RobotCBF(SimpleRobotDynamics(x0=np.array([50, 350])), (0, 255, 0), vel=3) + static_robot = RobotCBF(SimpleRobotDynamics(x0=np.array([120, 200])), (0, 0, 255), vel=0) + patrol_robot1 = RobotCBF(SimpleRobotDynamics(x0=np.array([230, 300])), (0, 0, 255), vel=1) + patrol_robot2 = RobotCBF(SimpleRobotDynamics(x0=np.array([300, 70])), (0, 0, 255), vel=1) collision_objects = [static_robot, patrol_robot1, patrol_robot2] # init control configs @@ -71,7 +71,7 @@ def run(): cbf_alphas.append(cbf_alphas.pop(0)) # roll left if e.key == pygame.K_r: # reset - auto_robot.x, auto_robot.y = 50, 350 + ego_robot.x, ego_robot.y = 50, 350 static_robot.x, static_robot.y = 120, 200 patrol_robot1.x, patrol_robot1.y = 230, 300 patrol_robot2.x, patrol_robot2.y = 300, 70 @@ -81,7 +81,7 @@ def run(): # move robots static_robot.control(None) - auto_robot.control( + ego_robot.control( pressed_key, use_cbf=use_cbf, cbf_alpha=cbf_alphas[0], @@ -91,7 +91,7 @@ def run(): patrol_robot1.control( direction_patrol_robot1, use_cbf=use_cbf_patrol_robots, - collision_objects=[auto_robot, static_robot, patrol_robot2] + collision_objects=[ego_robot, static_robot, patrol_robot2] if use_cbf_patrol_robots else [], force_direction_unchanged=cbf_force_direction_unchanged, @@ -99,24 +99,24 @@ def run(): patrol_robot2.control( direction_patrol_robot2, use_cbf=use_cbf_patrol_robots, - collision_objects=[auto_robot, static_robot, patrol_robot1] + collision_objects=[ego_robot, static_robot, patrol_robot1] if use_cbf_patrol_robots else [], force_direction_unchanged=cbf_force_direction_unchanged, ) # detect collision - auto_robot.detect_collision(collision_objects=collision_objects) + ego_robot.detect_collision(collision_objects=collision_objects) # draw robots screen.fill((0, 0, 0)) draw_robot(screen, static_robot) draw_robot(screen, patrol_robot1, draw_filtered_command=use_cbf_patrol_robots) draw_robot(screen, patrol_robot2, draw_filtered_command=use_cbf_patrol_robots) - draw_robot(screen, auto_robot, draw_filtered_command=use_cbf) + draw_robot(screen, ego_robot, draw_filtered_command=use_cbf) # draw texts - if auto_robot.is_collided: + if ego_robot.is_collided: screen.blit(text_surface, (230, 350)) if use_cbf: screen.blit(cbf_on_text_surface, (0, 0)) diff --git a/demo_codegen_osqp.py b/demo_codegen_osqp.py deleted file mode 100644 index ee3f6c8..0000000 --- a/demo_codegen_osqp.py +++ /dev/null @@ -1,282 +0,0 @@ -import sys -import pygame -import numpy as np -from scipy import sparse -import osqp - - -class Robot: - def __init__(self, x0=0, y0=0, color=(0, 255, 0), vel=3, size=30): - self.x = x0 # pos - self.y = y0 # pos - self.dx = 0 # control - self.dy = 0 # control - self.nominal_dx = 0 # user control - self.nominal_dy = 0 # user control - self.vel = vel - self.color = color - self.size = size - self.arrow_size = 10 - self.is_collided = False - - def control( - self, key=None, use_cbf=False, cbf_alpha=1e-1, penalty_slack=10, collision_objects=[] - ): - dx, dy = 0, 0 - self.nominal_dx, self.nominal_dy = 0, 0 - if key is not None: - if key == pygame.K_LEFT: - dx = -self.vel - elif key == pygame.K_RIGHT: - dx = self.vel - elif key == pygame.K_UP: - dy = -self.vel - elif key == pygame.K_DOWN: - dy = self.vel - self.nominal_dx, self.nominal_dy = dx, dy - - if use_cbf: - h = [] - coeffs = [] - for obj in collision_objects: - h.append((self.x - obj.x) ** 2 + (self.y - obj.y) ** 2 - (self.size * 2.3) ** 2) - coeffs.append([2 * self.x - 2 * obj.x, 2 * self.y - 2 * obj.y, penalty_slack]) - # Define problem data - # P = sparse.csc_matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) - q = np.array([-dx, -dy, 0]) - A = sparse.csc_matrix([c for c in coeffs] + [[1, 0, 0], [0, 1, 0], [0, 0, 1]]) - l = np.array([-cbf_alpha * h_ for h_ in h] + [-self.vel, -self.vel, -np.inf]) - u = np.array([np.inf for _ in h] + [self.vel, self.vel, np.inf]) - - emosqp.update_lower_bound(l) - emosqp.update_upper_bound(u) - emosqp.update_lin_cost(q) - emosqp.update_A(A.data, None, 0) - - # Solve problem - x, y, status_val, iter, run_time = emosqp.solve() - dx, dy, _ = x - - # update xy positions - self.x += dx - self.y += dy - self.dx, self.dy = dx, dy - - def detect_collision(self, collision_objects=[]): - self.is_collided = False - if len(collision_objects) > 0: - for obj in collision_objects: - dist = np.linalg.norm(np.array([self.x, self.y] - np.array([obj.x, obj.y]))) - if dist <= (self.size * 2): - self.is_collided = True - - def draw( - self, - screen, - draw_filtered_command=False, - color1=(175, 175, 175), - color2=(255, 0, 0), - ): - pygame.draw.circle(screen, self.color, (self.x, self.y), self.size) - - # draw arrows - if self.dx != 0: - pygame.draw.line( - screen, - color1, - (self.x, self.y), - (self.x + 20 * self.nominal_dx, self.y), - width=10, - ) - pygame.draw.polygon( - screen, - color1, - [ - (self.x + 20 * self.nominal_dx, self.y + self.arrow_size), - (self.x + 20 * self.nominal_dx, self.y - self.arrow_size), - (self.x + (self.arrow_size + 20) * self.nominal_dx, self.y), - ], - ) - if self.dy != 0: - pygame.draw.line( - screen, - color1, - (self.x, self.y), - (self.x, self.y + 20 * self.nominal_dy), - width=10, - ) - pygame.draw.polygon( - screen, - color1, - [ - (self.x + self.arrow_size, self.y + 20 * self.nominal_dy), - (self.x - self.arrow_size, self.y + 20 * self.nominal_dy), - (self.x, self.y + (self.arrow_size + 20) * self.nominal_dy), - ], - ) - - # draw output of safety filter - if draw_filtered_command: - if self.dx != 0: - pygame.draw.line( - screen, - color2, - (self.x, self.y), - (self.x + 20 * self.dx, self.y), - width=10, - ) - pygame.draw.polygon( - screen, - color2, - [ - (self.x + 20 * self.dx, self.y + self.arrow_size), - (self.x + 20 * self.dx, self.y - self.arrow_size), - (self.x + (self.arrow_size + 20) * self.dx, self.y), - ], - ) - if self.dy != 0: - pygame.draw.line( - screen, - color2, - (self.x, self.y), - (self.x, self.y + 20 * self.dy), - width=10, - ) - pygame.draw.polygon( - screen, - color2, - [ - (self.x + self.arrow_size, self.y + 20 * self.dy), - (self.x - self.arrow_size, self.y + 20 * self.dy), - (self.x, self.y + (self.arrow_size + 20) * self.dy), - ], - ) - - -def run(): - # inits - pygame.init() - pygame.font.init() - screen = pygame.display.set_mode((340, 400)) - pygame.display.set_caption("Control barrier function demo") - pygame.key.set_repeat(10) - - # init robots - auto_robot = Robot(50, 350, (0, 255, 0), vel=3) - static_robot = Robot(120, 200, (0, 0, 255)) - patrol_robot1 = Robot(230, 300, (0, 0, 255), vel=1) - patrol_robot2 = Robot(300, 70, (0, 0, 255), vel=1) - collision_objects = [static_robot, patrol_robot1, patrol_robot2] - - # control configs - count = 0 - use_cbf = False - cbf_alphas = [1e-1, 1e-2, 1] - direction_patrol_robot1 = pygame.K_UP - direction_patrol_robot2 = pygame.K_LEFT - - # set fonts - default_font = pygame.font.get_fonts()[0] - font = pygame.font.SysFont(default_font, 30) # - text_surface = font.render("Collide!", False, (255, 255, 255)) - cbf_font = pygame.font.SysFont(default_font, 20) - cbf_on_text_surface = cbf_font.render("CBF ON (press x to turn off)", False, (0, 255, 125)) - cbf_off_text_surface = cbf_font.render("CBF OFF (press x to turn on)", False, (255, 0, 125)) - - # main loop - while True: - count += 1 - - # flip moving direction of patrol robot - if count % 150 == 0: - direction_patrol_robot1 = ( - pygame.K_DOWN if (direction_patrol_robot1 == pygame.K_UP) else pygame.K_UP - ) - direction_patrol_robot2 = ( - pygame.K_RIGHT if (direction_patrol_robot2 == pygame.K_LEFT) else pygame.K_LEFT - ) - count = 0 if count >= 1e10 else count - - # get user input - pressed_key = None - for e in pygame.event.get(): - if e.type == pygame.QUIT: - sys.exit() - if e.type == pygame.KEYDOWN: - pressed_key = e.key - if e.type == pygame.KEYUP: - if e.key == pygame.K_x: - use_cbf = not use_cbf - if e.key == pygame.K_z: - cbf_alphas.append(cbf_alphas.pop(0)) # roll left - - # move robots - auto_robot.control( - pressed_key, - use_cbf=use_cbf, - cbf_alpha=cbf_alphas[0], - collision_objects=collision_objects, - ) - static_robot.control() - patrol_robot1.control(direction_patrol_robot1) - patrol_robot2.control(direction_patrol_robot2) - - # detect collision - auto_robot.detect_collision(collision_objects=collision_objects) - - # draw robots - screen.fill((0, 0, 0)) - static_robot.draw(screen) - patrol_robot1.draw(screen) - patrol_robot2.draw(screen) - auto_robot.draw(screen, draw_filtered_command=use_cbf) - - # draw texts - if auto_robot.is_collided: - screen.blit(text_surface, (230, 350)) - if use_cbf: - screen.blit(cbf_on_text_surface, (0, 0)) - tmp_render = cbf_font.render( - f"CBF alpha: {cbf_alphas[0]} (press z to change)", - False, - (0, 255, 125), - ) - screen.blit( - tmp_render, - (0, 20), - ) - else: - screen.blit(cbf_off_text_surface, (0, 0)) - - # render and wait - pygame.display.update() - pygame.time.wait(10) - - -def code_gen(): - h = [] - coeffs = [] - for obj in range(3): - h.append(0) - coeffs.append([1, 1, 10]) - # Define problem data - P = sparse.csc_matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) - q = np.array([0, 0, 0]) - A = sparse.csc_matrix([c for c in coeffs] + [[1, 0, 0], [0, 1, 0], [0, 0, 1]]) - l = np.array([-2.2639e3, -6.1128e3, -8.7328e3] + [-3, -3, -np.inf]) - u = np.array([np.inf for _ in h] + [3, 3, np.inf]) - - # Create an OSQP object - prob = osqp.OSQP() - prob.setup(P, q, A, l, u, verbose=False, time_limit=0) - - prob.codegen( - "code", parameters="matrices", python_ext_name="emosqp" # , project_type="MinGW Makefiles" - ) - - -if __name__ == "__main__": - code_gen() - import emosqp - - run() diff --git a/models/__pycache__/__init__.cpython-36.pyc b/models/__pycache__/__init__.cpython-36.pyc new file mode 100644 index 0000000000000000000000000000000000000000..0886a655481d571e4848bf32ca9402a5bab2c415 GIT binary patch literal 153 zcmXr!<>h*CMl_892p)q77+?f49Dul(1xTbY1T$zd`mJOr0tq9CUvAD;F`>n&Ma40} zjy|O^F8Rr&xv6<2#WC)gB_5?oG0skI@qq#H#ffRDC6)1MnK>n?MKQVgDXBTdG4b)4 bd6^~g@p=W7w>WHof~7gBb|CYMftUdR=V2$L literal 0 HcmV?d00001 diff --git a/models/__pycache__/i_model.cpython-36.pyc b/models/__pycache__/i_model.cpython-36.pyc new file mode 100644 index 0000000000000000000000000000000000000000..3ae115ecf60c6353e44a43a280f2f8cb8fe12f5d GIT binary patch literal 635 zcmZuu%}T>S5Z+DFSc>){;?1Lwix+PqqSjx~qKKCeShkrB2J)l38>BsIAHx^$5qt$7 zAy-enf+uGZTT#03O(wH5^L;bBy|U7i?`z2$W9$PlKQ4Ov2(y6(81RhcJmj36GH}41 zGH{=r&;jp;1)WdKVF9oEa%8NQvCWmeD5t%I~Z-u z*YRK{x;09h=;q*}ijK;-%2i=abeh_;Y7`9)PsH_=FfvhgERr-s;wUE#CQ3!)Y~7FR zt`KRFS|NzfIA|E3tpC-zHOpD7w2L*CJ>eP}wWxJk*Bn k_aihb?()9dM23M|yK)q_#dN=@xbE(@#C!BmjpAkJ8*cHA1ONa4 literal 0 HcmV?d00001 diff --git a/models/__pycache__/models.cpython-36.pyc b/models/__pycache__/models.cpython-36.pyc new file mode 100644 index 0000000000000000000000000000000000000000..2c8977d4228eee25c83607dc353462b5d3536ffd GIT binary patch literal 1743 zcmaJ>TW=#Z6!v8%B-# zWxexsREY#kgj)?N*HfXwX2Q=FcG##hem3yG+svX$(5m@%*KFoI|2R9Dv{zaNk zGxf5(EUWWbk>u&fXbN2KKLi*=zGXL`Lf4kfwDn9nS2ob9s&z4ffVzf%2K@}W`50h9 zC#0eic1ssT@C5_(797xDcz}T{A;X}55U$003__u9-VK@g0sZ(oW7rfRXuzJdn|Jx zaO8Xv=$-RLFjuJH!HRHmG5;&nPhi7!(%pJ0lS+wcqLbV_1B-?{ZNoQzwkh-vRzO}c z&kdm4(H`Dcc(arcwk-tQ<~qaiwh+J8N!DmwMcc5DiPp)?`tFsnJS{4Xsn-}qSI`>6 zp)tSu2;mq3Q?8#Pye%9z!rQ3vTW{P%6rPJOo4qC7Y-o}eq#|A{p$N4IBvhe5A_`Jj0a0J9l}66aIPuDtWo(P= zYI#atk@x}q1N;?!!o2d7zrYjcjM;6IE{x^RXU;jE`OanLS+Cbl|J;jza|!v2yfqw< zkKrw!14triNma>e8Jaa-Pb3>P+ji+Abhdw1Qh;*cTL!_(xYaX^FJtKj43oDRO zz>WJVPm0_)FO#w|&S^5K0&4o-7gD9)<(1NLG*J_DUJW0FV|dHo0hA>WGC5Ry>L?c; zPjczJcBL!58#nY(4;ue9lP$S*qEmKFmqhR-19X-g&|Nx!o+Pk09RIB-OX|`S|4PZ^Vy3h%$;*H5YM`Lbb*j7Oos?jQ(ggH!UHOJgJ1h zjYcnU`L881_zrMtYX2)-c1b$txe5S5SCQZ156FshvT`6b8hFfnLk7ulg`ju z_c3YisOeIS=@$F-BDRAa2mD@#tRk^Q?&XtS4&mQb9DAGMxLoHD%em(LF-#=V-d_YDnzg*+?>n*O7&w{@IL8Y zJ(E$T#4OTLR^pZodD@0&@nlo#@B9IE#X2{D*J`kX=M}CjC4^}U0f)IxQSJ)ya~-9P z#dfsua~WwJ&5dg>8N-vj(pY+pS+otUF&!G~s}B(#Az;b%`v`9u$4Yn_H6|aKlz5<8 z^w1mdAwT4Y9cu};jGNclY~E1t;G#?Ni?_DH#lwDc2OTBIU!@u^Q~Ud-9ZlTW{k;6rPJOX>RSNDcg1zBoHr_P=x@2gjT4~iUp}eK(#Mcq>-B$w~l-XV_P&) zcxqpf_yPL|_$&N`dF82pfhW!xZ?fBNU@U(=bI$S1cP=xZ_ImB~uf6zpmyo~78^Zzl z7~b+3fFzQpB&6_VDa&}sGbeO1H*_i5c9t$mJmsv zR6^j6B?6&+>cO*kx*=mnEBAmITI(*jPcS1m@QbZzv$GGv5~fqYDP7VV`UN-$Xu&2U zOz(h)aj%6es(|T7Fw=Vo7-R#x=;6nO{x#CF50lbvMHf#tF?;2AkIuSSNc45F8VX$Z za4TF`Zv*@f3;hn*k>ZNM!6pW)STM4;Q^P@PQ}YQ3ODgFBriSdpIo-uMw{zOeAES*) zXK1bam^63Pv@6DRhmCs~+rf?l{-9M>kys)R@=33T@b4;)y-jgkt#gRwT=V`2CK5_1 z;V?GLhvYYAHEZsm!DGNL88CK68r#6q)(0Q~uXRrcPE&K_82Zy_Qe24Zl`wzU1@0C0 z$z$*>Jpkel=zuOZR>68=d(Pgj!MYzlfHhV%5GdK01V(3L5zIC!M6f2@9MJzt^;1~z zA?aQ{lToF_EYeX{;+740+J@2C5neZrmGCrbOg=Ix z@j$icp*P?|e#j3y))H(PH?OnVyrJO1jhEz?Z)}5$hyCU P%CC^D&0mWSS(p6_<}`9Z literal 0 HcmV?d00001 diff --git a/models/i_model.py b/models/i_model.py new file mode 100644 index 0000000..33684a2 --- /dev/null +++ b/models/i_model.py @@ -0,0 +1,9 @@ +from abc import abstractmethod + +class ModelInterface: + def __init__(self) -> None: + pass + + @abstractmethod + def foward(self): + raise NotImplementedError diff --git a/models.py b/models/robot_dynamics.py similarity index 93% rename from models.py rename to models/robot_dynamics.py index 4324ef1..bc6b7c6 100644 --- a/models.py +++ b/models/robot_dynamics.py @@ -2,8 +2,9 @@ import numpy as np from typing import Union, List, Dict +from models.i_model import ModelInterface -class SimpleRobotDynamics: +class SimpleRobotDynamics(ModelInterface): def __init__(self, x0: np.ndarray, xr: float = 0, yr: float = 0, size: int = 30) -> None: self.x = x0.astype(np.float64) # xy positoin of the robot, shape(2,) self.xr = xr # x position of the obstacle diff --git a/tests/__init__.py b/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/__pycache__/__init__.cpython-36.pyc b/tests/__pycache__/__init__.cpython-36.pyc new file mode 100644 index 0000000000000000000000000000000000000000..488c44adf22b420efa3b9dffefd135f908d8e88c GIT binary patch literal 152 zcmXr!<>eB%AezPi1dl-k3@`#24nSPY0whuxf*CX!{Z=v*frJsnFIQ))n9$5JnlFInB%$$#)Uj3BZ zZ7jn*6v|`d1@bU`fxYIWSIDU|!?mmxr0hb&r8pe&n;8xtwpxvU|J&|7_Za)i<{lTm z_uyxqfly5Glts4Xk$}EPoxU5noV{eqQSK+E+=TVKi4*xrO?jt6KY@NQVI1ZjvCyA_ z9d;P!OKU$#~%F50f>Y}TrpUZEAfd%j$JHrm8X2@ zd#a`aXnj>z4QOj>O*NqnR7+ihwyv(j;_JoM+r(sZ3;#A$NqWp-0erAU;C=X+n;baw_0c5oxh6?FUziR+;oGbIU)%3Y zqD}kg>Fd0GFg(ut$sjZBm)-1D{=U8c?1lW}cWL5Il8t4jn`Vh_XRud8Qj$@#I|BPA z$=;1>VASB!jGy0H#RiR@4KxL`eEgVyP}Kd%i}I;n)7VlqrI9$}5}qFQIDKgL=SHj(Nk4C@w(YpA%Ku1T9MC$EE^LBBD6 zgP-rLhAZ`Fe7KWYxPO7pGQNKxogwtdS0P_u(_1iKZ-WSZ8#?bys$sT_EEG0mKjIXO z%v{_WL`GB5;uY6VtGP(KLNz;{kHAKSsyw2W>H^uI4k@t(b!n5Wu~~|`Hbp;l=n;Dr zG=4INbsYgVK$KXKcOw6e(?u|!z)rAUTtVGl4RulhS{QVG*#J+cwPjeb8u zQJplF;n+CQ@SO_Mcj83a6|7mDUV{xhQ&_M+I}{x_X%|pT>3j)_6P&uT#Mr)TOA3tH zzNeg7{Jg^{9{edL(e!5!p`-Dj=pRr}xJoay@{x+OM2=z|_l>4-E@;|(E&NhtA!Rox zvK1@$U=f4sGv0)ks?LMev98QLgo+qQ30GYrWzmpW)$rE6lP+(%| zTPSX$xQF6uqx6M^Q9B?on+%qhfCo1^B&`2UqN((M2M literal 0 HcmV?d00001 diff --git a/tests/__pycache__/test_robot.cpython-36.pyc b/tests/__pycache__/test_robot.cpython-36.pyc new file mode 100644 index 0000000000000000000000000000000000000000..458c1c394f676bda2697f089c16fb2160aa3704d GIT binary patch literal 2757 zcma)8-EJF26rP#=@!E0Hq;1mD7WzZEu#h64NQkOJfuu=MDpQ(7NGnUj#khgf3Elb_6Yfl z%se)HZ$VQ(f)GT|nE3gd`V9Ikwi4U76UTQF*LRbeUrRjSqhyZ=OW1FTu&2zgL(dT| z^xTm28&k_)3Y((#6O$e2FHZ>t%Lk<8ox?7q-Ey?|dU!lcx4+rd?mmC{eD_e-_WA2? z_g)-od!HY@!r`6oerR`5p?oa(*n*}Wfe49D1p$y$u-~WwWBXPP!?(?LHGO+0OOo+H zl#Jrg?6@-?1WD9Y?H1Lp4AV>wx(ux0gKuox&{PA&DLo||q5;S$D$tV_)3yrZo@OVX z{7z&O7X7og{j_tWLa90jU%$*cJHu|4go9Ld_M-GfcH99c`S-853VLBW=DjFRL)l3~ z1uIZ;iILbGjkU*lG>B5pYY4!gqLYVLd9YcozJwn`Wbj`{{KO`G@Zt+_YsL>*hq)G$ z=%S{X@gf(?I8y5k<*$JhI9;N6a3zXjF{YGrh;7KZZg8FqMHb_Clk=amAkJsx3fPpZ zC@^$00(X|^w+l@nD?PemnUOAZst2R81x-BzF-w8B1d?wU3CTBH^c_(X9*n!9E*kKz zi6zm5wL|q@6&@VC=6eL%lr5TomM!=!JGn;#<_@VJ;NfQDJ&Cx(4e^FbKU#C)X;x zn`ZVB>MHSw8t`E=$ft~6^esWLe+jj%mjVU+O`l_;FQ3VsHHt#xnzBiRVr_D6j@xLA|@P z=_#!h8@&%|>NW^Mn{7wl&f^Z$jL{GlQw5(aq70fZjp#Gbyx+UWWZ>VJ~QuD zNz6xUGsG=J=H Date: Wed, 10 Apr 2024 08:35:03 +0900 Subject: [PATCH 2/3] Update README.md --- README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/README.md b/README.md index 12f3d98..2f1ba6b 100644 --- a/README.md +++ b/README.md @@ -24,10 +24,9 @@ Run `demo.py` to start the simulation. A simple version w/ code generation is pr python demo.py # a PyGame window will be spawn ``` -Unit test can be executed through the following command +Unit test can be executed through the following command. ```bash python -m unittest -# ........... # ---------------------------------------------------------------------- # Ran 11 tests in 0.000s # OK From a80eea81d9c147a0a5e382594d096fc05f8b1b13 Mon Sep 17 00:00:00 2001 From: shaoanlu Date: Wed, 10 Apr 2024 08:35:48 +0900 Subject: [PATCH 3/3] delete cache --- controllers/__pycache__/__init__.cpython-36.pyc | Bin 158 -> 0 bytes .../__pycache__/i_controller.cpython-36.pyc | Bin 814 -> 0 bytes controllers/__pycache__/robot.cpython-36.pyc | Bin 6018 -> 0 bytes controllers/__pycache__/robot_cbf.cpython-36.pyc | Bin 6327 -> 0 bytes models/__pycache__/__init__.cpython-36.pyc | Bin 153 -> 0 bytes models/__pycache__/i_model.cpython-36.pyc | Bin 635 -> 0 bytes models/__pycache__/models.cpython-36.pyc | Bin 1743 -> 0 bytes models/__pycache__/robot.cpython-36.pyc | Bin 1797 -> 0 bytes models/__pycache__/robot_dynamics.cpython-36.pyc | Bin 1806 -> 0 bytes tests/__pycache__/__init__.cpython-36.pyc | Bin 152 -> 0 bytes tests/__pycache__/test_models.cpython-36.pyc | Bin 2157 -> 0 bytes tests/__pycache__/test_robot.cpython-36.pyc | Bin 2757 -> 0 bytes 12 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 controllers/__pycache__/__init__.cpython-36.pyc delete mode 100644 controllers/__pycache__/i_controller.cpython-36.pyc delete mode 100644 controllers/__pycache__/robot.cpython-36.pyc delete mode 100644 controllers/__pycache__/robot_cbf.cpython-36.pyc delete mode 100644 models/__pycache__/__init__.cpython-36.pyc delete mode 100644 models/__pycache__/i_model.cpython-36.pyc delete mode 100644 models/__pycache__/models.cpython-36.pyc delete mode 100644 models/__pycache__/robot.cpython-36.pyc delete mode 100644 models/__pycache__/robot_dynamics.cpython-36.pyc delete mode 100644 tests/__pycache__/__init__.cpython-36.pyc delete mode 100644 tests/__pycache__/test_models.cpython-36.pyc delete mode 100644 tests/__pycache__/test_robot.cpython-36.pyc diff --git a/controllers/__pycache__/__init__.cpython-36.pyc b/controllers/__pycache__/__init__.cpython-36.pyc deleted file mode 100644 index 7ea5aae4eb97186f2901efacdb2a2ddb76d97e6a..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 158 zcmXr!<>h*CMl_892p)q77+?f49Dul(1xTbY1T$zd`mJOr0tq9CU*67EF`>n&Ma40} zjy|O^F8Rr&xv6<2#WC)gB_5?oG0skI@qq#H#ffRDC6)1MnK>n?MKQ_wc_l^pIXNJ0 f@$s2?nI-Y@dIgoYIBatBQ%ZAE?Ld|k12F>t&6g?M diff --git a/controllers/__pycache__/i_controller.cpython-36.pyc b/controllers/__pycache__/i_controller.cpython-36.pyc deleted file mode 100644 index 73ff3f1d09fb02c8092d198454ea5b41654100f5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 814 zcma)4!EV$r5Vhm%Zn|tMq>2k5E^x`E2e=`GfCWk=w16ruQ6wvyaYQ61X~)i?J<|OQ zet>V`4|3(iFK}WeX}1*$5|(B%p3Ka9e)f~y-TA{GcZ$1Gh@axdQWU=+a}UUY0BJ=f z6Dh@^fB@7+fVv2h0K#JtPkvF2h-J5zt)0g#_m%Nq8pt_)wYCEvQ^tryf}nN@!8ZvC z!cCYc6T$?h8*w3%3B@yr`gjVGKz6$QWm9`>tTlL4dxJ%mn-!r>(fAhYoQ}{h{<1B} zTu2TC^~=?FrWiV73!G54cfUU@pQfLkL6;uCeBY+8o4l<|?Ol3U`giR*UA}s&KYi3L zD~vzWMQI5*&9_8cTI%cF!O2+{X`y0IqNrC>4gU8mLXJqOr71ujpvg0;6eGOxdOf znWj~-P&X=BZdP(UQ^|9yQn;q@EYCeqcy3><6gj)Cl=F{)TUjf!#Ejamh&p* z6+H8_;uY~4cuV~FLzAE2bHILtpXBool*&=Qz)#_OjGyMu;Cq}u%NOxI!Jp&L<2%P+ z;5NP|fsX;hiw_w;!(Rf1d0qmOUQG%S5Tt&0Q#)2I1nfxlTejVlThz#XgB&QpxM^}Go0!cR{*nI!HTps zxt6QtoX7%SqIS9{y(HHPn?5Ax;D1u@i98009DxE2&-;;s)doKIc%rvOxK%c=W-q8o zbm>SrhlW2=#2h|fzJKN7=0}kyqRrcvuXi?A!)mALwc=>=nje4A+1kAF{#EDZx)ZrI zFYY-tKZrfC3EW|v!liaEu^h*5`LW|11;&WbDjAbazk4Og@1@b;DlwiO`i?nCYwIi9 z$~Jg-nz*8-Tq&z5Q&_jm#0d?kFr$~`5`shrU!K@8FQ&7B|UC z$VP~ZYM<>hru0?uHvV8y+oUTD_GG}m%ze1ZmW395A4-L>9n~Ud(Oz_s$nY@VSo;|-41%RZD|ZJwX|b?*fBlf*a%ZQjC^8FB#K0) z1W?YTD!fPy&k-Q6BVHh26L^sT8PY_-lgyCf3YsI5s*){eIZbEjt1@~m&C>RYsp=2M zA2NWkX3k?iI3xL|0y@NJP4bAXmGucr4y<~%;k^t@B&$?A+1}#N%u{|%v8752phrg; zFNSs>lOS1Nt*iTNK%!M4V2#8*UR0vC+KJ%`;r3Ezkb(Yv>NHqs4R*LAolNGo6ELL_@aRw`Ad1aus zE}%t4%L=vN-^Ra&|JC6LtyMycj+R?;gofatNl`#;9rLN`nn}zLo$KGfx*-EVlKIfN z^@D34Y$W=J&PVIiw|e8w+C+XM-bh^=GiaJ4x4M5GrAv*epQ@HN@qbBB2}E!mL~ zlO0j~l1BVDRIob*#>v31%z(^O${7Z$_cMK~Z|r9_jX`#h8{`LtK@t53oI94d7i(n5 zscJ$SjDEJC>*xE0evzwO`S=v-TIC-CLPC)wVn5&^xm)T8fuuc8q8@UVFV;w*UFF-`nT^ z{hMF^1#}wJaQh9_zYF2+dJ~~;Ks$z#w%x687i!|R>@ClR z*J?@y;xr!}@+vtPisuBdff#u>yPvQiVV#P4xmzwJi%xZ9eNMxH4B+CuS}k&TgLe_6 zxmWXB9%ZaU&O3WE4xT6eS0R+YOO0~|xr1fZ@CC{?0}tD{AS zhei^`*+UtB`{a95FKg_l379r)$-sLvuGZNC4tq?wU%G?jtgqEnnQE1_MD2=1?Fo58 z5i%9EWWp;b)l7qUaTIf1;Wnf6EMsDCUW=bQbv|r7RrV z2Lm)fVg!BFzFybrVBNrqiKQ$LrsR<@$hNGUh4A$NL2(sqn}vZh!az(NyM# z&avFUJcLY|hc+DnP2E>z4RrVAO606O&A@q_sEkG%YxT&8CFpTjbun)s^&44mw*QE5Nv5ynd(NRgJVkDkANZqv%mDw-U`}*wz zDR5a$)TIjvt4@jc%iN{Fk7C5%_Pd{vTnMMB?7caZ>ui6pbSVfS@#tMtMzasCin2Y* zC}lb%S5+FYpDJJln^T@#n^?uzL~S@YGRh=grtb1sdKEo?BuweT4pAXRl6xojB&p;E z$Rf87?j9$V*U+12h>@rhA@L4@!>0p5bpC`gi_ZVE8*vTDW^d>7r;zyu+YuiS_OIE_ zpC@#1MkD@Ja@DAc^dc61SF%OCj@Be2V_&C<3c`Q0)0F6CdR#6(-Di)TNP7CZ2vpKl zi|l0C!b*abM26}F<=M&yROG|Peww61ZbkZO zj*7f6Qes#E`PLm>{J48vdhFE!nL4s!!12+xPHiw~bW-l0x`P`;7^J!P+i}3Y9)@jXRU+)v8@3w+_V8Seyo4@#qO#1b5HW4KO3Vzee!EIhef%qDzf7c^NWSV$G9}gEKwP0qq|v!Y#tY&tf;?6>WjD!D z?!`zI)0;E8l=v%#Mub2yOvH&CJ4ffk`P83G4}PSj_Xrec(?muSxH(k>2~<$vC@;|U z817g-$4PRIgGkj0s9tcK`yDq(dju&#Vjv|6yu_#lp&OIM$AXSyiG^II?TNTI*&=Rs zy%@nxkkKXjR){MKoqU;i6%FEL0(8n1X9&DRfC8Z))fMXmCjMjMtmsaY?m{9mD@(^E z3Y#w$^nzZ}OJ;h(K+s;mHG^&yt#4UJtT{omkeV5-PP5%hrDH@@A9WI*cpD%w<6awL yPnVk>&Z8w6wIjKALHAqY4Fcx~oF_ntXRR}_E~RXI7eP3+LJOioQ46E{5beJ>G8-8H diff --git a/controllers/__pycache__/robot_cbf.cpython-36.pyc b/controllers/__pycache__/robot_cbf.cpython-36.pyc deleted file mode 100644 index a987a210a821e7d4bb299d776940b54719cce77b..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6327 zcmbVQON<;x8Sd)GJbE79_1lh+@K{@kfk5!%yIEznVvLE13N(W05S2u>U_( zdQ3|hQ&I?L(@NOj)tQtA4Mm*jqmXVXRhdJDR3+WjMR=4a_LWl8)y@)Y(+hfH)&|c2 z(|ChlkJK?t+_kZklzay5dkGy!+xaP>2ZoTq6{r;?iAl}_BuCs{mwT~rCuTU-esp-U_51)8Dz>y#slUk7V3a5vD#2}Q?Do=KM~s==E|2r?=DJ2tI$}LZ5^NPVXP9FAmJNGa+w|OS~*7#sqIL|nvqUy z6y>cUi?t11p2QwlNgL^%^Jvl0vPdoXckplEe`!3z=v2{SqUDwvVIZOyN=8s=H}_Ok z&L_DK+-vV%xuZfwlK;TH_5RiO?j+_1?%f;Iw{-o(<*8sNTvB?Nv1%`GUc`>RixQE- zXsn=Kd&f7&wwx!pwZ|T6xv9u^D7w;n()P>;kS!KbE~7+$0009t*fs-8^CPlur4+Dt zbC@3%hSqj|#Tpey_NX)}k1FU#DBdW@`>{cOAXRIqjx{U}?O|zH9#*){4Q_s%hqpLP zaeQwG=g%fmfxVIgcb;$ z6jlkU49XRi_2qhZq1U!EelB}KqZh!(IPscrDAqcSu-*1L+*utsQ5b9rztd8kO+@(c zBl-)GB)0u6j~t0Z;XV*;J3~1^O5KiXxT&x)Leuc#< z=(fH7mr|mc=hfKHKT{vS)66D(v?ip;{%B~=`Ihs&UjK~K8{BjJNHKQo7ahOT?Ztpj z-y!}F9IuZNQbb(}XvhNxyEhGjV+<;mIh&sJJ;-7gLACEagjV6Bd?B4CBBYWbkxNaNM*g%ZTo;^rd zkg#4|f2UuoCMVp+#QNMd7cxLR5Y1-f@-^N^jOU{1cLe3YW6rxs20VEHZYVK{v32wM z?VC61RwUwHH_4@uAu(xGVx_#M&^&dqQ?FN>+Y^#zxxs|3C8eEfVmBmZWC?pmzD|f2 z>BoSECd}RIs86A5^#=svjcD8yZbrK8LgMnBq567p7(P14UGCFN<chk7+nH;XJO!sFc9-c_AoC$Cpf5@if$p|iiJDcS8Msg&DwEMxHrhgJ_B* z@4#J{;--Eoz>wiXDliW4O$$m>Tvl45GW;z4C5w2jZ*v{S(S~ZM&#s+owajqGFGbov!a> z0Mc_ho zl5FdCBWfTgiKX9HY>_pzCV3Swdu>#ZGqihcg%vpL3SQKXpt@Ir`f4p3HWUN+jUyW4%zCoLbq623?m>HykW}k z_pd3(zBD3BM;Z;79i>~;275-*tgq+kY<3jBL7mX(j8@>rL2ZM{qnz%&v3>vwc-}D7{6gFf`lt4#q5JCbL<9@GN3TPWF!f)R=o5PC5gd@PSpdja`RSIBrU-6igtJsH4Je}O0G=4 zf(A)<0O}YmU!>Y80u(Y+_T@V~J*AT#%bNtI&P3IemQ$)Rj}no)DVVs$VTUVavusw) zYA(IuAm|(^7;|QI-?XLrR&{>zXTmP32PEOD%_UZ+*X|C|RajBON1bFW-vUT-@t_Os zNVjxI1^;M4sg29W_wR~N^`qkx15_wl@F%wj_>&ePdyN2H(aEy}76=giS({7>7gLE` WqAMtMZ(4xhQS>q^o3who#(x2u&S3cf diff --git a/models/__pycache__/__init__.cpython-36.pyc b/models/__pycache__/__init__.cpython-36.pyc deleted file mode 100644 index 0886a655481d571e4848bf32ca9402a5bab2c415..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 153 zcmXr!<>h*CMl_892p)q77+?f49Dul(1xTbY1T$zd`mJOr0tq9CUvAD;F`>n&Ma40} zjy|O^F8Rr&xv6<2#WC)gB_5?oG0skI@qq#H#ffRDC6)1MnK>n?MKQVgDXBTdG4b)4 bd6^~g@p=W7w>WHof~7gBb|CYMftUdR=V2$L diff --git a/models/__pycache__/i_model.cpython-36.pyc b/models/__pycache__/i_model.cpython-36.pyc deleted file mode 100644 index 3ae115ecf60c6353e44a43a280f2f8cb8fe12f5d..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 635 zcmZuu%}T>S5Z+DFSc>){;?1Lwix+PqqSjx~qKKCeShkrB2J)l38>BsIAHx^$5qt$7 zAy-enf+uGZTT#03O(wH5^L;bBy|U7i?`z2$W9$PlKQ4Ov2(y6(81RhcJmj36GH}41 zGH{=r&;jp;1)WdKVF9oEa%8NQvCWmeD5t%I~Z-u z*YRK{x;09h=;q*}ijK;-%2i=abeh_;Y7`9)PsH_=FfvhgERr-s;wUE#CQ3!)Y~7FR zt`KRFS|NzfIA|E3tpC-zHOpD7w2L*CJ>eP}wWxJk*Bn k_aihb?()9dM23M|yK)q_#dN=@xbE(@#C!BmjpAkJ8*cHA1ONa4 diff --git a/models/__pycache__/models.cpython-36.pyc b/models/__pycache__/models.cpython-36.pyc deleted file mode 100644 index 2c8977d4228eee25c83607dc353462b5d3536ffd..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1743 zcmaJ>TW=#Z6!v8%B-# zWxexsREY#kgj)?N*HfXwX2Q=FcG##hem3yG+svX$(5m@%*KFoI|2R9Dv{zaNk zGxf5(EUWWbk>u&fXbN2KKLi*=zGXL`Lf4kfwDn9nS2ob9s&z4ffVzf%2K@}W`50h9 zC#0eic1ssT@C5_(797xDcz}T{A;X}55U$003__u9-VK@g0sZ(oW7rfRXuzJdn|Jx zaO8Xv=$-RLFjuJH!HRHmG5;&nPhi7!(%pJ0lS+wcqLbV_1B-?{ZNoQzwkh-vRzO}c z&kdm4(H`Dcc(arcwk-tQ<~qaiwh+J8N!DmwMcc5DiPp)?`tFsnJS{4Xsn-}qSI`>6 zp)tSu2;mq3Q?8#Pye%9z!rQ3vTW{P%6rPJOo4qC7Y-o}eq#|A{p$N4IBvhe5A_`Jj0a0J9l}66aIPuDtWo(P= zYI#atk@x}q1N;?!!o2d7zrYjcjM;6IE{x^RXU;jE`OanLS+Cbl|J;jza|!v2yfqw< zkKrw!14triNma>e8Jaa-Pb3>P+ji+Abhdw1Qh;*cTL!_(xYaX^FJtKj43oDRO zz>WJVPm0_)FO#w|&S^5K0&4o-7gD9)<(1NLG*J_DUJW0FV|dHo0hA>WGC5Ry>L?c; zPjczJcBL!58#nY(4;ue9lP$S*qEmKFmqhR-19X-g&|Nx!o+Pk09RIB-OX|`S|4PZ^Vy3h%$;*H5YM`Lbb*j7Oos?jQ(ggH!UHOJgJ1h zjYcnU`L881_zrMtYX2)-c1b$txe5S5SCQZ156FshvT`6b8hFfnLk7ulg`ju z_c3YisOeIS=@$F-BDRAa2mD@#tRk^Q?&XtS4&mQb9DAGMxLoHD%em(LF-#=V-d_YDnzg*+?>n*O7&w{@IL8Y zJ(E$T#4OTLR^pZodD@0&@nlo#@B9IE#X2{D*J`kX=M}CjC4^}U0f)IxQSJ)ya~-9P z#dfsua~WwJ&5dg>8N-vj(pY+pS+otUF&!G~s}B(#Az;b%`v`9u$4Yn_H6|aKlz5<8 z^w1mdAwT4Y9cu};jGNclY~E1t;G#?Ni?_DH#lwDc2OTBIU!@u^Q~Ud-9ZlTW{k;6rPJOX>RSNDcg1zBoHr_P=x@2gjT4~iUp}eK(#Mcq>-B$w~l-XV_P&) zcxqpf_yPL|_$&N`dF82pfhW!xZ?fBNU@U(=bI$S1cP=xZ_ImB~uf6zpmyo~78^Zzl z7~b+3fFzQpB&6_VDa&}sGbeO1H*_i5c9t$mJmsv zR6^j6B?6&+>cO*kx*=mnEBAmITI(*jPcS1m@QbZzv$GGv5~fqYDP7VV`UN-$Xu&2U zOz(h)aj%6es(|T7Fw=Vo7-R#x=;6nO{x#CF50lbvMHf#tF?;2AkIuSSNc45F8VX$Z za4TF`Zv*@f3;hn*k>ZNM!6pW)STM4;Q^P@PQ}YQ3ODgFBriSdpIo-uMw{zOeAES*) zXK1bam^63Pv@6DRhmCs~+rf?l{-9M>kys)R@=33T@b4;)y-jgkt#gRwT=V`2CK5_1 z;V?GLhvYYAHEZsm!DGNL88CK68r#6q)(0Q~uXRrcPE&K_82Zy_Qe24Zl`wzU1@0C0 z$z$*>Jpkel=zuOZR>68=d(Pgj!MYzlfHhV%5GdK01V(3L5zIC!M6f2@9MJzt^;1~z zA?aQ{lToF_EYeX{;+740+J@2C5neZrmGCrbOg=Ix z@j$icp*P?|e#j3y))H(PH?OnVyrJO1jhEz?Z)}5$hyCU P%CC^D&0mWSS(p6_<}`9Z diff --git a/tests/__pycache__/__init__.cpython-36.pyc b/tests/__pycache__/__init__.cpython-36.pyc deleted file mode 100644 index 488c44adf22b420efa3b9dffefd135f908d8e88c..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 152 zcmXr!<>eB%AezPi1dl-k3@`#24nSPY0whuxf*CX!{Z=v*frJsnFIQ))n9$5JnlFInB%$$#)Uj3BZ zZ7jn*6v|`d1@bU`fxYIWSIDU|!?mmxr0hb&r8pe&n;8xtwpxvU|J&|7_Za)i<{lTm z_uyxqfly5Glts4Xk$}EPoxU5noV{eqQSK+E+=TVKi4*xrO?jt6KY@NQVI1ZjvCyA_ z9d;P!OKU$#~%F50f>Y}TrpUZEAfd%j$JHrm8X2@ zd#a`aXnj>z4QOj>O*NqnR7+ihwyv(j;_JoM+r(sZ3;#A$NqWp-0erAU;C=X+n;baw_0c5oxh6?FUziR+;oGbIU)%3Y zqD}kg>Fd0GFg(ut$sjZBm)-1D{=U8c?1lW}cWL5Il8t4jn`Vh_XRud8Qj$@#I|BPA z$=;1>VASB!jGy0H#RiR@4KxL`eEgVyP}Kd%i}I;n)7VlqrI9$}5}qFQIDKgL=SHj(Nk4C@w(YpA%Ku1T9MC$EE^LBBD6 zgP-rLhAZ`Fe7KWYxPO7pGQNKxogwtdS0P_u(_1iKZ-WSZ8#?bys$sT_EEG0mKjIXO z%v{_WL`GB5;uY6VtGP(KLNz;{kHAKSsyw2W>H^uI4k@t(b!n5Wu~~|`Hbp;l=n;Dr zG=4INbsYgVK$KXKcOw6e(?u|!z)rAUTtVGl4RulhS{QVG*#J+cwPjeb8u zQJplF;n+CQ@SO_Mcj83a6|7mDUV{xhQ&_M+I}{x_X%|pT>3j)_6P&uT#Mr)TOA3tH zzNeg7{Jg^{9{edL(e!5!p`-Dj=pRr}xJoay@{x+OM2=z|_l>4-E@;|(E&NhtA!Rox zvK1@$U=f4sGv0)ks?LMev98QLgo+qQ30GYrWzmpW)$rE6lP+(%| zTPSX$xQF6uqx6M^Q9B?on+%qhfCo1^B&`2UqN((M2M diff --git a/tests/__pycache__/test_robot.cpython-36.pyc b/tests/__pycache__/test_robot.cpython-36.pyc deleted file mode 100644 index 458c1c394f676bda2697f089c16fb2160aa3704d..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2757 zcma)8-EJF26rP#=@!E0Hq;1mD7WzZEu#h64NQkOJfuu=MDpQ(7NGnUj#khgf3Elb_6Yfl z%se)HZ$VQ(f)GT|nE3gd`V9Ikwi4U76UTQF*LRbeUrRjSqhyZ=OW1FTu&2zgL(dT| z^xTm28&k_)3Y((#6O$e2FHZ>t%Lk<8ox?7q-Ey?|dU!lcx4+rd?mmC{eD_e-_WA2? z_g)-od!HY@!r`6oerR`5p?oa(*n*}Wfe49D1p$y$u-~WwWBXPP!?(?LHGO+0OOo+H zl#Jrg?6@-?1WD9Y?H1Lp4AV>wx(ux0gKuox&{PA&DLo||q5;S$D$tV_)3yrZo@OVX z{7z&O7X7og{j_tWLa90jU%$*cJHu|4go9Ld_M-GfcH99c`S-853VLBW=DjFRL)l3~ z1uIZ;iILbGjkU*lG>B5pYY4!gqLYVLd9YcozJwn`Wbj`{{KO`G@Zt+_YsL>*hq)G$ z=%S{X@gf(?I8y5k<*$JhI9;N6a3zXjF{YGrh;7KZZg8FqMHb_Clk=amAkJsx3fPpZ zC@^$00(X|^w+l@nD?PemnUOAZst2R81x-BzF-w8B1d?wU3CTBH^c_(X9*n!9E*kKz zi6zm5wL|q@6&@VC=6eL%lr5TomM!=!JGn;#<_@VJ;NfQDJ&Cx(4e^FbKU#C)X;x zn`ZVB>MHSw8t`E=$ft~6^esWLe+jj%mjVU+O`l_;FQ3VsHHt#xnzBiRVr_D6j@xLA|@P z=_#!h8@&%|>NW^Mn{7wl&f^Z$jL{GlQw5(aq70fZjp#Gbyx+UWWZ>VJ~QuD zNz6xUGsG=J=H