From b7d16e74a587d826b70d52ecafa502bbce1450c0 Mon Sep 17 00:00:00 2001 From: MagDish <2717360869@qq.com> Date: Mon, 6 May 2024 19:33:33 +0800 Subject: [PATCH] first commit --- .idea/.gitignore | 8 + .idea/FCPCodebase.iml | 12 + .idea/inspectionProfiles/Project_Default.xml | 19 + .../inspectionProfiles/profiles_settings.xml | 6 + .idea/misc.xml | 7 + .idea/modules.xml | 8 + .idea/vcs.xml | 6 + LICENSE | 674 +++++++++ One_vs_One.py | 36 + Run_Full_Team.py | 13 + Run_One_vs_One.py | 14 + Run_Player.py | 18 + Run_Utils.py | 96 ++ T_vs_T.py | 35 + agent/Agent.py | 271 ++++ agent/Agent_Penalty.py | 88 ++ agent/Base_Agent.py | 47 + behaviors/Behavior.py | 183 +++ behaviors/Head.py | 105 ++ behaviors/Poses.py | 97 ++ behaviors/Slot_Engine.py | 110 ++ behaviors/custom/Basic_Kick/Basic_Kick.py | 74 + behaviors/custom/Dribble/Dribble.py | 207 +++ behaviors/custom/Dribble/Env.py | 181 +++ behaviors/custom/Dribble/dribble_R0.pkl | Bin 0 -> 24148 bytes behaviors/custom/Dribble/dribble_R1.pkl | Bin 0 -> 24148 bytes behaviors/custom/Dribble/dribble_R2.pkl | Bin 0 -> 24148 bytes behaviors/custom/Dribble/dribble_R3.pkl | Bin 0 -> 24148 bytes behaviors/custom/Dribble/dribble_R4.pkl | Bin 0 -> 24148 bytes behaviors/custom/Fall/Fall.py | 43 + behaviors/custom/Fall/fall.pkl | Bin 0 -> 28854 bytes behaviors/custom/Get_Up/Get_Up.py | 68 + behaviors/custom/Kick_8/Kick_8.py | 75 + behaviors/custom/Step/Step.py | 59 + behaviors/custom/Step/Step_Generator.py | 75 + behaviors/custom/Walk/Env.py | 201 +++ behaviors/custom/Walk/Walk.py | 83 ++ behaviors/custom/Walk/walk_R0.pkl | Bin 0 -> 20820 bytes behaviors/custom/Walk/walk_R1_R3.pkl | Bin 0 -> 20820 bytes behaviors/custom/Walk/walk_R2.pkl | Bin 0 -> 20820 bytes behaviors/custom/Walk/walk_R4.pkl | Bin 0 -> 20820 bytes behaviors/slot/common/Dive_Left.xml | 71 + behaviors/slot/common/Dive_Right.xml | 71 + behaviors/slot/common/Flip.xml | 12 + behaviors/slot/common/Squat.xml | 24 + behaviors/slot/r0/Get_Up_Back.xml | 148 ++ behaviors/slot/r0/Get_Up_Front.xml | 220 +++ behaviors/slot/r0/Kick_Motion.xml | 21 + behaviors/slot/r1/Get_Up_Back.xml | 148 ++ behaviors/slot/r1/Get_Up_Front.xml | 220 +++ behaviors/slot/r1/Kick_Motion.xml | 21 + behaviors/slot/r2/Get_Up_Back.xml | 148 ++ behaviors/slot/r2/Get_Up_Front.xml | 220 +++ behaviors/slot/r2/Kick_Motion.xml | 21 + behaviors/slot/r3/Get_Up_Back.xml | 148 ++ behaviors/slot/r3/Get_Up_Front.xml | 220 +++ behaviors/slot/r3/Kick_Motion.xml | 22 + behaviors/slot/r4/Get_Up_Back.xml | 148 ++ behaviors/slot/r4/Get_Up_Front.xml | 220 +++ behaviors/slot/r4/Kick_Motion.xml | 21 + bundle/bundle.sh | 83 ++ communication/Radio.py | 306 ++++ communication/Server_Comm.py | 285 ++++ communication/World_Parser.py | 430 ++++++ cpp/a_star/Makefile | 14 + cpp/a_star/a_star.cpp | 918 ++++++++++++ cpp/a_star/a_star.h | 26 + cpp/a_star/debug_main.cc | 37 + cpp/a_star/expansion_groups.h | 6 + cpp/a_star/expansion_groups.py | 60 + cpp/a_star/lib_main.cpp | 43 + cpp/ball_predictor/Makefile | 14 + cpp/ball_predictor/ball_predictor.cpp | 104 ++ cpp/ball_predictor/ball_predictor.h | 10 + cpp/ball_predictor/debug_main.cc | 54 + cpp/ball_predictor/lib_main.cpp | 100 ++ cpp/localization/Field.cpp | 532 +++++++ cpp/localization/Field.h | 503 +++++++ cpp/localization/FieldNoise.cpp | 104 ++ cpp/localization/FieldNoise.h | 91 ++ cpp/localization/Geometry.cpp | 344 +++++ cpp/localization/Geometry.h | 71 + cpp/localization/Line6f.cpp | 232 +++ cpp/localization/Line6f.h | 185 +++ cpp/localization/LocalizerV2.cpp | 1249 +++++++++++++++++ cpp/localization/LocalizerV2.h | 379 +++++ cpp/localization/Makefile | 15 + cpp/localization/Matrix4D.cpp | 303 ++++ cpp/localization/Matrix4D.h | 224 +++ cpp/localization/RobovizLogger.cpp | 131 ++ cpp/localization/RobovizLogger.h | 58 + cpp/localization/Singleton.h | 20 + cpp/localization/Vector3f.cpp | 197 +++ cpp/localization/Vector3f.h | 187 +++ cpp/localization/World.h | 56 + cpp/localization/debug_main.cc | 196 +++ cpp/localization/lib_main.cpp | 182 +++ cpp/localization/robovizdraw.h | 208 +++ example.py | 30 + kill.sh | 2 + logs/Logger.py | 48 + math_ops/Inverse_Kinematics.py | 242 ++++ math_ops/Math_Ops.py | 361 +++++ math_ops/Matrix_3x3.py | 350 +++++ math_ops/Matrix_4x4.py | 440 ++++++ math_ops/Neural_Network.py | 28 + scripts/commons/Script.py | 300 ++++ scripts/commons/Server.py | 60 + scripts/commons/Train_Base.py | 494 +++++++ scripts/commons/UI.py | 302 ++++ scripts/gyms/Basic_Run.py | 281 ++++ scripts/gyms/Fall.py | 214 +++ scripts/utils/Beam.py | 60 + scripts/utils/Behaviors.py | 55 + scripts/utils/Drawings.py | 35 + scripts/utils/Dribble.py | 54 + scripts/utils/Fwd_Kinematics.py | 115 ++ scripts/utils/Get_Up.py | 45 + scripts/utils/IMU.py | 179 +++ scripts/utils/Inv_Kinematics.py | 146 ++ scripts/utils/Joints.py | 151 ++ scripts/utils/Kick.py | 53 + scripts/utils/Localization.py | 103 ++ scripts/utils/Pathfinding.py | 203 +++ scripts/utils/Radio_Localization.py | 97 ++ scripts/utils/Server.py | 148 ++ scripts/utils/Team_Communication.py | 73 + start.sh | 9 + start_debug.sh | 9 + start_fat_proxy.sh | 9 + start_fat_proxy_debug.sh | 9 + start_penalty.sh | 8 + start_penalty_debug.sh | 8 + world/Robot.py | 535 +++++++ world/World.py | 430 ++++++ world/commons/Body_Part.py | 7 + world/commons/Draw.py | 345 +++++ world/commons/Joint_Info.py | 24 + world/commons/Other_Robot.py | 28 + world/commons/Path_Manager.py | 583 ++++++++ world/commons/robots/nao0.xml | 165 +++ world/commons/robots/nao1.xml | 161 +++ world/commons/robots/nao2.xml | 164 +++ world/commons/robots/nao3.xml | 161 +++ world/commons/robots/nao4.xml | 175 +++ 145 files changed, 20499 insertions(+) create mode 100644 .idea/.gitignore create mode 100644 .idea/FCPCodebase.iml create mode 100644 .idea/inspectionProfiles/Project_Default.xml create mode 100644 .idea/inspectionProfiles/profiles_settings.xml create mode 100644 .idea/misc.xml create mode 100644 .idea/modules.xml create mode 100644 .idea/vcs.xml create mode 100644 LICENSE create mode 100644 One_vs_One.py create mode 100644 Run_Full_Team.py create mode 100644 Run_One_vs_One.py create mode 100644 Run_Player.py create mode 100644 Run_Utils.py create mode 100644 T_vs_T.py create mode 100644 agent/Agent.py create mode 100644 agent/Agent_Penalty.py create mode 100644 agent/Base_Agent.py create mode 100644 behaviors/Behavior.py create mode 100644 behaviors/Head.py create mode 100644 behaviors/Poses.py create mode 100644 behaviors/Slot_Engine.py create mode 100644 behaviors/custom/Basic_Kick/Basic_Kick.py create mode 100644 behaviors/custom/Dribble/Dribble.py create mode 100644 behaviors/custom/Dribble/Env.py create mode 100644 behaviors/custom/Dribble/dribble_R0.pkl create mode 100644 behaviors/custom/Dribble/dribble_R1.pkl create mode 100644 behaviors/custom/Dribble/dribble_R2.pkl create mode 100644 behaviors/custom/Dribble/dribble_R3.pkl create mode 100644 behaviors/custom/Dribble/dribble_R4.pkl create mode 100644 behaviors/custom/Fall/Fall.py create mode 100644 behaviors/custom/Fall/fall.pkl create mode 100644 behaviors/custom/Get_Up/Get_Up.py create mode 100644 behaviors/custom/Kick_8/Kick_8.py create mode 100644 behaviors/custom/Step/Step.py create mode 100644 behaviors/custom/Step/Step_Generator.py create mode 100644 behaviors/custom/Walk/Env.py create mode 100644 behaviors/custom/Walk/Walk.py create mode 100644 behaviors/custom/Walk/walk_R0.pkl create mode 100644 behaviors/custom/Walk/walk_R1_R3.pkl create mode 100644 behaviors/custom/Walk/walk_R2.pkl create mode 100644 behaviors/custom/Walk/walk_R4.pkl create mode 100644 behaviors/slot/common/Dive_Left.xml create mode 100644 behaviors/slot/common/Dive_Right.xml create mode 100644 behaviors/slot/common/Flip.xml create mode 100644 behaviors/slot/common/Squat.xml create mode 100644 behaviors/slot/r0/Get_Up_Back.xml create mode 100644 behaviors/slot/r0/Get_Up_Front.xml create mode 100644 behaviors/slot/r0/Kick_Motion.xml create mode 100644 behaviors/slot/r1/Get_Up_Back.xml create mode 100644 behaviors/slot/r1/Get_Up_Front.xml create mode 100644 behaviors/slot/r1/Kick_Motion.xml create mode 100644 behaviors/slot/r2/Get_Up_Back.xml create mode 100644 behaviors/slot/r2/Get_Up_Front.xml create mode 100644 behaviors/slot/r2/Kick_Motion.xml create mode 100644 behaviors/slot/r3/Get_Up_Back.xml create mode 100644 behaviors/slot/r3/Get_Up_Front.xml create mode 100644 behaviors/slot/r3/Kick_Motion.xml create mode 100644 behaviors/slot/r4/Get_Up_Back.xml create mode 100644 behaviors/slot/r4/Get_Up_Front.xml create mode 100644 behaviors/slot/r4/Kick_Motion.xml create mode 100755 bundle/bundle.sh create mode 100644 communication/Radio.py create mode 100644 communication/Server_Comm.py create mode 100644 communication/World_Parser.py create mode 100644 cpp/a_star/Makefile create mode 100644 cpp/a_star/a_star.cpp create mode 100644 cpp/a_star/a_star.h create mode 100644 cpp/a_star/debug_main.cc create mode 100644 cpp/a_star/expansion_groups.h create mode 100644 cpp/a_star/expansion_groups.py create mode 100644 cpp/a_star/lib_main.cpp create mode 100644 cpp/ball_predictor/Makefile create mode 100644 cpp/ball_predictor/ball_predictor.cpp create mode 100644 cpp/ball_predictor/ball_predictor.h create mode 100644 cpp/ball_predictor/debug_main.cc create mode 100644 cpp/ball_predictor/lib_main.cpp create mode 100644 cpp/localization/Field.cpp create mode 100644 cpp/localization/Field.h create mode 100644 cpp/localization/FieldNoise.cpp create mode 100644 cpp/localization/FieldNoise.h create mode 100644 cpp/localization/Geometry.cpp create mode 100644 cpp/localization/Geometry.h create mode 100644 cpp/localization/Line6f.cpp create mode 100644 cpp/localization/Line6f.h create mode 100644 cpp/localization/LocalizerV2.cpp create mode 100644 cpp/localization/LocalizerV2.h create mode 100644 cpp/localization/Makefile create mode 100644 cpp/localization/Matrix4D.cpp create mode 100644 cpp/localization/Matrix4D.h create mode 100644 cpp/localization/RobovizLogger.cpp create mode 100644 cpp/localization/RobovizLogger.h create mode 100644 cpp/localization/Singleton.h create mode 100644 cpp/localization/Vector3f.cpp create mode 100644 cpp/localization/Vector3f.h create mode 100644 cpp/localization/World.h create mode 100644 cpp/localization/debug_main.cc create mode 100644 cpp/localization/lib_main.cpp create mode 100644 cpp/localization/robovizdraw.h create mode 100644 example.py create mode 100755 kill.sh create mode 100644 logs/Logger.py create mode 100644 math_ops/Inverse_Kinematics.py create mode 100644 math_ops/Math_Ops.py create mode 100644 math_ops/Matrix_3x3.py create mode 100644 math_ops/Matrix_4x4.py create mode 100644 math_ops/Neural_Network.py create mode 100644 scripts/commons/Script.py create mode 100644 scripts/commons/Server.py create mode 100644 scripts/commons/Train_Base.py create mode 100644 scripts/commons/UI.py create mode 100644 scripts/gyms/Basic_Run.py create mode 100644 scripts/gyms/Fall.py create mode 100644 scripts/utils/Beam.py create mode 100644 scripts/utils/Behaviors.py create mode 100644 scripts/utils/Drawings.py create mode 100644 scripts/utils/Dribble.py create mode 100644 scripts/utils/Fwd_Kinematics.py create mode 100644 scripts/utils/Get_Up.py create mode 100644 scripts/utils/IMU.py create mode 100644 scripts/utils/Inv_Kinematics.py create mode 100644 scripts/utils/Joints.py create mode 100644 scripts/utils/Kick.py create mode 100644 scripts/utils/Localization.py create mode 100644 scripts/utils/Pathfinding.py create mode 100644 scripts/utils/Radio_Localization.py create mode 100644 scripts/utils/Server.py create mode 100644 scripts/utils/Team_Communication.py create mode 100755 start.sh create mode 100755 start_debug.sh create mode 100755 start_fat_proxy.sh create mode 100755 start_fat_proxy_debug.sh create mode 100755 start_penalty.sh create mode 100755 start_penalty_debug.sh create mode 100644 world/Robot.py create mode 100644 world/World.py create mode 100644 world/commons/Body_Part.py create mode 100644 world/commons/Draw.py create mode 100644 world/commons/Joint_Info.py create mode 100644 world/commons/Other_Robot.py create mode 100644 world/commons/Path_Manager.py create mode 100644 world/commons/robots/nao0.xml create mode 100644 world/commons/robots/nao1.xml create mode 100644 world/commons/robots/nao2.xml create mode 100644 world/commons/robots/nao3.xml create mode 100644 world/commons/robots/nao4.xml diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..13566b8 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Editor-based HTTP Client requests +/httpRequests/ +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml diff --git a/.idea/FCPCodebase.iml b/.idea/FCPCodebase.iml new file mode 100644 index 0000000..8b8c395 --- /dev/null +++ b/.idea/FCPCodebase.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/Project_Default.xml b/.idea/inspectionProfiles/Project_Default.xml new file mode 100644 index 0000000..d42459f --- /dev/null +++ b/.idea/inspectionProfiles/Project_Default.xml @@ -0,0 +1,19 @@ + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..a33389a --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,7 @@ + + + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..cb91ae6 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..35eb1dd --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..f288702 --- /dev/null +++ b/LICENSE @@ -0,0 +1,674 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/One_vs_One.py b/One_vs_One.py new file mode 100644 index 0000000..dca9fd8 --- /dev/null +++ b/One_vs_One.py @@ -0,0 +1,36 @@ +from agent.Base_Agent import Base_Agent as Agent +from math_ops.Math_Ops import Math_Ops as M +from scripts.commons.Script import Script + +script = Script() +a = script.args + +# Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name +p1 = Agent(a.i, a.p, a.m, a.u, a.r, a.t) +p2 = Agent(a.i, a.p, a.m, a.u, a.r, "Opponent") +players = [p1,p2] + +p1.scom.unofficial_beam((-3,0,p1.world.robot.beam_height), 0) +p2.scom.unofficial_beam((-3,0,p2.world.robot.beam_height), 0) + +getting_up = [False]*2 + +while True: + for i in range(len(players)): + p = players[i] + w = p.world + + player_2d = w.robot.loc_head_position[:2] + ball_2d = w.ball_abs_pos[:2] + goal_dir = M.vector_angle( (15,0)-player_2d ) # Goal direction + + if p.behavior.is_ready("Get_Up") or getting_up[i]: + getting_up[i] = not p.behavior.execute("Get_Up") # True on completion + else: + p.behavior.execute("Basic_Kick", goal_dir) + + p.scom.commit_and_send( w.robot.get_command() ) + + # all players must commit and send before the server updates + p1.scom.receive() + p2.scom.receive() \ No newline at end of file diff --git a/Run_Full_Team.py b/Run_Full_Team.py new file mode 100644 index 0000000..74587ef --- /dev/null +++ b/Run_Full_Team.py @@ -0,0 +1,13 @@ +from scripts.commons.Script import Script +script = Script() # Initialize: load config file, parse arguments, build cpp modules +a = script.args + +from agent.Agent import Agent + +# Args: Server IP, Agent Port, Monitor Port, Uniform No., Team name, Enable Log, Enable Draw +team_args = ((a.i, a.p, a.m, u, a.t, True, True) for u in range(1,12)) +script.batch_create(Agent,team_args) + +while True: + script.batch_execute_agent() + script.batch_receive() diff --git a/Run_One_vs_One.py b/Run_One_vs_One.py new file mode 100644 index 0000000..98aed42 --- /dev/null +++ b/Run_One_vs_One.py @@ -0,0 +1,14 @@ +from scripts.commons.Script import Script +script = Script() # Initialize: load config file, parse arguments, build cpp modules +a = script.args + +from agent.Agent import Agent + +# Args: Server IP, Agent Port, Monitor Port, Uniform No., Team name, Enable Log, Enable Draw +script.batch_create(Agent, ((a.i, a.p, a.m, a.u, a.t, True, True),)) #one player for home team +script.batch_create(Agent, ((a.i, a.p, a.m, a.u, "Opponent", True, True),)) #one player for away team + + +while True: + script.batch_execute_agent() + script.batch_receive() diff --git a/Run_Player.py b/Run_Player.py new file mode 100644 index 0000000..46419b8 --- /dev/null +++ b/Run_Player.py @@ -0,0 +1,18 @@ +from scripts.commons.Script import Script +script = Script(cpp_builder_unum=1) # Initialize: load config file, parse arguments, build cpp modules +a = script.args + +if a.P: # penalty shootout + from agent.Agent_Penalty import Agent +else: # normal agent + from agent.Agent import Agent + +# Args: Server IP, Agent Port, Monitor Port, Uniform No., Team name, Enable Log, Enable Draw, Wait for Server, is magmaFatProxy +if a.D: # debug mode + player = Agent(a.i, a.p, a.m, a.u, a.t, True, True, False, a.F) +else: + player = Agent(a.i, a.p, None, a.u, a.t, False, False, False, a.F) + +while True: + player.think_and_send() + player.scom.receive() diff --git a/Run_Utils.py b/Run_Utils.py new file mode 100644 index 0000000..66f7e21 --- /dev/null +++ b/Run_Utils.py @@ -0,0 +1,96 @@ +def main(): + + from scripts.commons.Script import Script + script = Script() #Initialize: load config file, parse arguments, build cpp modules (warns the user about inconsistencies before choosing a test script) + + # Allows using local version of StableBaselines3 (e.g. https://github.com/m-abr/Adaptive-Symmetry-Learning) + # place the 'stable-baselines3' folder in the parent directory of this project + import sys + from os.path import dirname, abspath, join + sys.path.insert( 0, join( dirname(dirname( abspath(__file__) )), "stable-baselines3") ) + + from scripts.commons.UI import UI + from os.path import isfile, join, realpath, dirname + from os import listdir, getcwd + from importlib import import_module + + _cwd = realpath( join(getcwd(), dirname(__file__))) + gyms_path = _cwd + "/scripts/gyms/" + utils_path = _cwd + "/scripts/utils/" + exclusions = ["__init__.py"] + + utils = sorted([f[:-3] for f in listdir(utils_path) if isfile(join(utils_path, f)) and f.endswith(".py") and f not in exclusions], key=lambda x: (x != "Server", x)) + gyms = sorted([f[:-3] for f in listdir(gyms_path ) if isfile(join(gyms_path , f)) and f.endswith(".py") and f not in exclusions]) + + while True: + _, col_idx, col = UI.print_table( [utils, gyms], ["Demos & Tests & Utils","Gyms"], cols_per_title=[2,1], numbering=[True]*2, prompt='Choose script (ctrl+c to exit): ' ) + + is_gym = False + if col == 0: + chosen = ("scripts.utils." , utils[col_idx]) + elif col == 1: + chosen = ("scripts.gyms." , gyms[col_idx]) + is_gym = True + + cls_name = chosen[1] + mod = import_module(chosen[0]+chosen[1]) + + ''' + An imported script should not automatically execute the main code because: + - Multiprocessing methods, such as 'forkserver' and 'spawn', will execute the main code in every child + - The script can only be called once unless it is reloaded + ''' + if not is_gym: + ''' + Utils receive a script support object with user-defined arguments and useful methods + Each util must implement an execute() method, which may or may not return + ''' + from world.commons.Draw import Draw + from agent.Base_Agent import Base_Agent + obj = getattr(mod,cls_name)(script) + try: + obj.execute() # Util may return normally or through KeyboardInterrupt + except KeyboardInterrupt: + print("\nctrl+c pressed, returning...\n") + Draw.clear_all() # clear all drawings + Base_Agent.terminate_all() # close all server sockets + monitor socket + script.players = [] # clear list of players created through batch commands + del obj + + else: + ''' + Gyms must implement a class Train() which is initialized with user-defined arguments and implements: + train() - method to run the optimization and save a new model + test(folder_dir, folder_name, model_file) - method to load an existing model and test it + ''' + from scripts.commons.Train_Base import Train_Base + + print("\nBefore using GYMS, make sure all server parameters are set correctly") + print("(sync mode should be 'On', real time should be 'Off', cheats should be 'On', ...)") + print("To change these parameters go to the previous menu, and select Server\n") + print("Also, GYMS start their own servers, so don't run any server manually") + + while True: + try: + idx = UI.print_table([["Train","Test","Retrain"]], numbering=[True], prompt='Choose option (ctrl+c to return): ')[0] + except KeyboardInterrupt: + print() + break + + if idx == 0: + mod.Train(script).train(dict()) + else: + model_info = Train_Base.prompt_user_for_model() + if model_info is not None and idx == 1: + mod.Train(script).test(model_info) + elif model_info is not None: + mod.Train(script).train(model_info) + + +# allow child processes to bypass this file +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + print("\nctrl+c pressed, exiting...") + exit() \ No newline at end of file diff --git a/T_vs_T.py b/T_vs_T.py new file mode 100644 index 0000000..2d6c1fd --- /dev/null +++ b/T_vs_T.py @@ -0,0 +1,35 @@ +from agent.Base_Agent import Base_Agent as Agent +from math_ops.Math_Ops import Math_Ops as M +from scripts.commons.Script import Script + +script = Script() +a = script.args + +# Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name +# This could be done in 1 line, but calling batch_create twice enhances readability + +script.batch_create(Agent, ((a.i,a.p,a.m,u+1,a.r,"Home") for u in range(3)) ) +script.batch_create(Agent, ((a.i,a.p,a.m,u+1,a.r,"Away") for u in range(3)) ) + +players = script.players +p_num = len(players) # total number of players +script.batch_unofficial_beam( (-3, 4.5-abs(3*i-7.5), 0.5, 0) for i in range(p_num) ) + +getting_up = [False]*p_num + +while True: + for i in range(p_num): + p = players[i] + w = p.world + + player_2d = w.robot.loc_head_position[:2] + ball_2d = w.ball_abs_pos[:2] + goal_dir = M.vector_angle( (15,0)-player_2d ) # Goal direction + + if p.behavior.is_ready("Get_Up") or getting_up[i]: + getting_up[i] = not p.behavior.execute("Get_Up") # True on completion + else: + p.behavior.execute("Basic_Kick", goal_dir) + + script.batch_commit_and_send() + script.batch_receive() \ No newline at end of file diff --git a/agent/Agent.py b/agent/Agent.py new file mode 100644 index 0000000..c5500e2 --- /dev/null +++ b/agent/Agent.py @@ -0,0 +1,271 @@ +from agent.Base_Agent import Base_Agent +from math_ops.Math_Ops import Math_Ops as M +import math +import numpy as np + + +class Agent(Base_Agent): + def __init__(self, host:str, agent_port:int, monitor_port:int, unum:int, + team_name:str, enable_log, enable_draw, wait_for_server=True, is_fat_proxy=False) -> None: + + # define robot type + robot_type = (0,1,1,1,2,3,3,3,4,4,4)[unum-1] + + # Initialize base agent + # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name, Enable Log, Enable Draw, play mode correction, Wait for Server, Hear Callback + super().__init__(host, agent_port, monitor_port, unum, robot_type, team_name, enable_log, enable_draw, True, wait_for_server, None) + + self.enable_draw = enable_draw + self.state = 0 # 0-Normal, 1-Getting up, 2-Kicking + self.kick_direction = 0 + self.kick_distance = 0 + self.fat_proxy_cmd = "" if is_fat_proxy else None + self.fat_proxy_walk = np.zeros(3) # filtered walk parameters for fat proxy + + self.init_pos = ([-14,0],[-9,-5],[-9,0],[-9,5],[-5,-5],[-5,0],[-5,5],[-1,-6],[-1,-2.5],[-1,2.5],[-1,6])[unum-1] # initial formation + + + def beam(self, avoid_center_circle=False): + r = self.world.robot + pos = self.init_pos[:] # copy position list + self.state = 0 + + # Avoid center circle by moving the player back + if avoid_center_circle and np.linalg.norm(self.init_pos) < 2.5: + pos[0] = -2.3 + + if np.linalg.norm(pos - r.loc_head_position[:2]) > 0.1 or self.behavior.is_ready("Get_Up"): + self.scom.commit_beam(pos, M.vector_angle((-pos[0],-pos[1]))) # beam to initial position, face coordinate (0,0) + else: + if self.fat_proxy_cmd is None: # normal behavior + self.behavior.execute("Zero_Bent_Knees_Auto_Head") + else: # fat proxy behavior + self.fat_proxy_cmd += "(proxy dash 0 0 0)" + self.fat_proxy_walk = np.zeros(3) # reset fat proxy walk + + + def move(self, target_2d=(0,0), orientation=None, is_orientation_absolute=True, + avoid_obstacles=True, priority_unums=[], is_aggressive=False, timeout=3000): + ''' + Walk to target position + + Parameters + ---------- + target_2d : array_like + 2D target in absolute coordinates + orientation : float + absolute or relative orientation of torso, in degrees + set to None to go towards the target (is_orientation_absolute is ignored) + is_orientation_absolute : bool + True if orientation is relative to the field, False if relative to the robot's torso + avoid_obstacles : bool + True to avoid obstacles using path planning (maybe reduce timeout arg if this function is called multiple times per simulation cycle) + priority_unums : list + list of teammates to avoid (since their role is more important) + is_aggressive : bool + if True, safety margins are reduced for opponents + timeout : float + restrict path planning to a maximum duration (in microseconds) + ''' + r = self.world.robot + + if self.fat_proxy_cmd is not None: # fat proxy behavior + self.fat_proxy_move(target_2d, orientation, is_orientation_absolute) # ignore obstacles + return + + if avoid_obstacles: + target_2d, _, distance_to_final_target = self.path_manager.get_path_to_target( + target_2d, priority_unums=priority_unums, is_aggressive=is_aggressive, timeout=timeout) + else: + distance_to_final_target = np.linalg.norm(target_2d - r.loc_head_position[:2]) + + self.behavior.execute("Walk", target_2d, True, orientation, is_orientation_absolute, distance_to_final_target) # Args: target, is_target_abs, ori, is_ori_abs, distance + + + + + + def kick(self, kick_direction=None, kick_distance=None, abort=False, enable_pass_command=False): + ''' + Walk to ball and kick + + Parameters + ---------- + kick_direction : float + kick direction, in degrees, relative to the field + kick_distance : float + kick distance in meters + abort : bool + True to abort. + The method returns True upon successful abortion, which is immediate while the robot is aligning itself. + However, if the abortion is requested during the kick, it is delayed until the kick is completed. + avoid_pass_command : bool + When False, the pass command will be used when at least one opponent is near the ball + + Returns + ------- + finished : bool + Returns True if the behavior finished or was successfully aborted. + ''' + + if self.min_opponent_ball_dist < 1.45 and enable_pass_command: + self.scom.commit_pass_command() + + self.kick_direction = self.kick_direction if kick_direction is None else kick_direction + self.kick_distance = self.kick_distance if kick_distance is None else kick_distance + + if self.fat_proxy_cmd is None: # normal behavior + return self.behavior.execute("Basic_Kick", self.kick_direction, abort) # Basic_Kick has no kick distance control + else: # fat proxy behavior + return self.fat_proxy_kick() + + + + + def think_and_send(self): + w = self.world + r = self.world.robot + my_head_pos_2d = r.loc_head_position[:2] + my_ori = r.imu_torso_orientation + ball_2d = w.ball_abs_pos[:2] + ball_vec = ball_2d - my_head_pos_2d + ball_dir = M.vector_angle(ball_vec) + ball_dist = np.linalg.norm(ball_vec) + ball_sq_dist = ball_dist * ball_dist # for faster comparisons + ball_speed = np.linalg.norm(w.get_ball_abs_vel(6)[:2]) + behavior = self.behavior + goal_dir = M.target_abs_angle(ball_2d,(15.05,0)) + path_draw_options = self.path_manager.draw_options + PM = w.play_mode + PM_GROUP = w.play_mode_group + + #--------------------------------------- 1. Preprocessing + + slow_ball_pos = w.get_predicted_ball_pos(0.5) # predicted future 2D ball position when ball speed <= 0.5 m/s + + # list of squared distances between teammates (including self) and slow ball (sq distance is set to 1000 in some conditions) + teammates_ball_sq_dist = [np.sum((p.state_abs_pos[:2] - slow_ball_pos) ** 2) # squared distance between teammate and ball + if p.state_last_update != 0 and (w.time_local_ms - p.state_last_update <= 360 or p.is_self) and not p.state_fallen + else 1000 # force large distance if teammate does not exist, or its state info is not recent (360 ms), or it has fallen + for p in w.teammates ] + + # list of squared distances between opponents and slow ball (sq distance is set to 1000 in some conditions) + opponents_ball_sq_dist = [np.sum((p.state_abs_pos[:2] - slow_ball_pos) ** 2) # squared distance between teammate and ball + if p.state_last_update != 0 and w.time_local_ms - p.state_last_update <= 360 and not p.state_fallen + else 1000 # force large distance if opponent does not exist, or its state info is not recent (360 ms), or it has fallen + for p in w.opponents ] + + min_teammate_ball_sq_dist = min(teammates_ball_sq_dist) + self.min_teammate_ball_dist = math.sqrt(min_teammate_ball_sq_dist) # distance between ball and closest teammate + self.min_opponent_ball_dist = math.sqrt(min(opponents_ball_sq_dist)) # distance between ball and closest opponent + + active_player_unum = teammates_ball_sq_dist.index(min_teammate_ball_sq_dist) + 1 + + + #--------------------------------------- 2. Decide action + + + + if PM == w.M_GAME_OVER: + pass + elif PM_GROUP == w.MG_ACTIVE_BEAM: + self.beam() + elif PM_GROUP == w.MG_PASSIVE_BEAM: + self.beam(True) # avoid center circle + elif self.state == 1 or (behavior.is_ready("Get_Up") and self.fat_proxy_cmd is None): + self.state = 0 if behavior.execute("Get_Up") else 1 # return to normal state if get up behavior has finished + elif PM == w.M_OUR_KICKOFF: + if r.unum == 9: + self.kick(120,3) # no need to change the state when PM is not Play On + else: + self.move(self.init_pos, orientation=ball_dir) # walk in place + elif PM == w.M_THEIR_KICKOFF: + self.move(self.init_pos, orientation=ball_dir) # walk in place + elif active_player_unum != r.unum: # I am not the active player + if r.unum == 1: # I am the goalkeeper + self.move(self.init_pos, orientation=ball_dir) # walk in place + else: + # compute basic formation position based on ball position + new_x = max(0.5,(ball_2d[0]+15)/15) * (self.init_pos[0]+15) - 15 + if self.min_teammate_ball_dist < self.min_opponent_ball_dist: + new_x = min(new_x + 3.5, 13) # advance if team has possession + self.move((new_x,self.init_pos[1]), orientation=ball_dir, priority_unums=[active_player_unum]) + + else: # I am the active player + path_draw_options(enable_obstacles=True, enable_path=True, use_team_drawing_channel=True) # enable path drawings for active player (ignored if self.enable_draw is False) + enable_pass_command = (PM == w.M_PLAY_ON and ball_2d[0]<6) + + if r.unum == 1 and PM_GROUP == w.MG_THEIR_KICK: # goalkeeper during their kick + self.move(self.init_pos, orientation=ball_dir) # walk in place + if PM == w.M_OUR_CORNER_KICK: + self.kick( -np.sign(ball_2d[1])*95, 5.5) # kick the ball into the space in front of the opponent's goal + # no need to change the state when PM is not Play On + elif self.min_opponent_ball_dist + 0.5 < self.min_teammate_ball_dist: # defend if opponent is considerably closer to the ball + if self.state == 2: # commit to kick while aborting + self.state = 0 if self.kick(abort=True) else 2 + else: # move towards ball, but position myself between ball and our goal + self.move(slow_ball_pos + M.normalize_vec((-16,0) - slow_ball_pos) * 0.2, is_aggressive=True) + else: + self.state = 0 if self.kick(goal_dir,9,False,enable_pass_command) else 2 + + path_draw_options(enable_obstacles=False, enable_path=False) # disable path drawings + + #--------------------------------------- 3. Broadcast + self.radio.broadcast() + + #--------------------------------------- 4. Send to server + if self.fat_proxy_cmd is None: # normal behavior + self.scom.commit_and_send( r.get_command() ) + else: # fat proxy behavior + self.scom.commit_and_send( self.fat_proxy_cmd.encode() ) + self.fat_proxy_cmd = "" + + #---------------------- annotations for debugging + if self.enable_draw: + d = w.draw + if active_player_unum == r.unum: + d.point(slow_ball_pos, 3, d.Color.pink, "status", False) # predicted future 2D ball position when ball speed <= 0.5 m/s + d.point(w.ball_2d_pred_pos[-1], 5, d.Color.pink, "status", False) # last ball prediction + d.annotation((*my_head_pos_2d, 0.6), "I've got it!" , d.Color.yellow, "status") + else: + d.clear("status") + + + + + #--------------------------------------- Fat proxy auxiliary methods + + + def fat_proxy_kick(self): + w = self.world + r = self.world.robot + ball_2d = w.ball_abs_pos[:2] + my_head_pos_2d = r.loc_head_position[:2] + + if np.linalg.norm(ball_2d - my_head_pos_2d) < 0.25: + # fat proxy kick arguments: power [0,10]; relative horizontal angle [-180,180]; vertical angle [0,70] + self.fat_proxy_cmd += f"(proxy kick 10 {M.normalize_deg( self.kick_direction - r.imu_torso_orientation ):.2f} 20)" + self.fat_proxy_walk = np.zeros(3) # reset fat proxy walk + return True + else: + self.fat_proxy_move(ball_2d-(-0.1,0), None, True) # ignore obstacles + return False + + + def fat_proxy_move(self, target_2d, orientation, is_orientation_absolute): + r = self.world.robot + + target_dist = np.linalg.norm(target_2d - r.loc_head_position[:2]) + target_dir = M.target_rel_angle(r.loc_head_position[:2], r.imu_torso_orientation, target_2d) + + if target_dist > 0.1 and abs(target_dir) < 8: + self.fat_proxy_cmd += (f"(proxy dash {100} {0} {0})") + return + + if target_dist < 0.1: + if is_orientation_absolute: + orientation = M.normalize_deg( orientation - r.imu_torso_orientation ) + target_dir = np.clip(orientation, -60, 60) + self.fat_proxy_cmd += (f"(proxy dash {0} {0} {target_dir:.1f})") + else: + self.fat_proxy_cmd += (f"(proxy dash {20} {0} {target_dir:.1f})") \ No newline at end of file diff --git a/agent/Agent_Penalty.py b/agent/Agent_Penalty.py new file mode 100644 index 0000000..b90b3e3 --- /dev/null +++ b/agent/Agent_Penalty.py @@ -0,0 +1,88 @@ +from agent.Base_Agent import Base_Agent +from math_ops.Math_Ops import Math_Ops as M +import numpy as np +import random + + +class Agent(Base_Agent): + def __init__(self, host:str, agent_port:int, monitor_port:int, unum:int, + team_name:str, enable_log, enable_draw, wait_for_server=True, is_fat_proxy=False) -> None: + + # define robot type + robot_type = 0 if unum == 1 else 4 # assume the goalkeeper uses uniform number 1 and the kicker uses any other number + + # Initialize base agent + # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name, Enable Log, Enable Draw, play mode correction, Wait for Server, Hear Callback + super().__init__(host, agent_port, monitor_port, unum, robot_type, team_name, enable_log, enable_draw, False, wait_for_server, None) + + self.enable_draw = enable_draw + self.state = 0 # 0-Normal, 1-Getting up, 2-Dive Left, 3-Dive Right, 4-Wait + + self.kick_dir = 0 # kick direction + self.reset_kick = True # when True, a new random kick direction is generated + + + def think_and_send(self): + w = self.world + r = self.world.robot + my_head_pos_2d = r.loc_head_position[:2] + my_ori = r.imu_torso_orientation + ball_2d = w.ball_abs_pos[:2] + ball_vec = ball_2d - my_head_pos_2d + ball_dir = M.vector_angle(ball_vec) + ball_dist = np.linalg.norm(ball_vec) + ball_speed = np.linalg.norm(w.get_ball_abs_vel(6)[:2]) + behavior = self.behavior + PM = w.play_mode + + #--------------------------------------- 1. Decide action + + if PM in [w.M_BEFORE_KICKOFF, w.M_THEIR_GOAL, w.M_OUR_GOAL]: # beam to initial position and wait + self.state = 0 + self.reset_kick = True + pos = (-14,0) if r.unum == 1 else (4.9,0) + if np.linalg.norm(pos - r.loc_head_position[:2]) > 0.1 or behavior.is_ready("Get_Up"): + self.scom.commit_beam(pos, 0) # beam to initial position + else: + behavior.execute("Zero_Bent_Knees") # wait + elif self.state == 2: # dive left + self.state = 4 if behavior.execute("Dive_Left") else 2 # change state to wait after skill has finished + elif self.state == 3: # dive right + self.state = 4 if behavior.execute("Dive_Right") else 3 # change state to wait after skill has finished + elif self.state == 4: # wait (after diving or during opposing kick) + pass + elif self.state == 1 or behavior.is_ready("Get_Up"): # if getting up or fallen + self.state = 0 if behavior.execute("Get_Up") else 1 # return to normal state if get up behavior has finished + elif PM == w.M_OUR_KICKOFF and r.unum == 1 or PM == w.M_THEIR_KICKOFF and r.unum != 1: + self.state = 4 # wait until next beam + elif r.unum == 1: # goalkeeper + y_coordinate = np.clip(ball_2d[1], -1.1, 1.1) + behavior.execute("Walk", (-14,y_coordinate), True, 0, True, None) # Args: target, is_target_abs, ori, is_ori_abs, distance + if ball_2d[0] < -10: + self.state = 2 if ball_2d[1] > 0 else 3 # dive to defend + else: # kicker + if PM == w.M_OUR_KICKOFF and ball_2d[0] > 5: # check ball position to make sure I see it + if self.reset_kick: + self.kick_dir = random.choice([-7.5,7.5]) + self.reset_kick = False + behavior.execute("Basic_Kick", self.kick_dir) + else: + behavior.execute("Zero_Bent_Knees") # wait + + #--------------------------------------- 2. Broadcast + self.radio.broadcast() + + #--------------------------------------- 3. Send to server + self.scom.commit_and_send( r.get_command() ) + + #---------------------- annotations for debugging + if self.enable_draw: + d = w.draw + if r.unum == 1: + d.annotation((*my_head_pos_2d, 0.8), "Goalkeeper" , d.Color.yellow, "status") + else: + d.annotation((*my_head_pos_2d, 0.8), "Kicker" , d.Color.yellow, "status") + if PM == w.M_OUR_KICKOFF: # draw arrow to indicate kick direction + d.arrow(ball_2d, ball_2d + 5*M.vector_from_angle(self.kick_dir), 0.4, 3, d.Color.cyan_light, "Target") + + diff --git a/agent/Base_Agent.py b/agent/Base_Agent.py new file mode 100644 index 0000000..650e203 --- /dev/null +++ b/agent/Base_Agent.py @@ -0,0 +1,47 @@ +from abc import abstractmethod +from behaviors.Behavior import Behavior +from communication.Radio import Radio +from communication.Server_Comm import Server_Comm +from communication.World_Parser import World_Parser +from logs.Logger import Logger +from math_ops.Inverse_Kinematics import Inverse_Kinematics +from world.commons.Path_Manager import Path_Manager +from world.World import World + +class Base_Agent(): + all_agents = [] + + def __init__(self, host:str, agent_port:int, monitor_port:int, unum:int, robot_type:int, team_name:str, enable_log:bool=True, + enable_draw:bool=True, apply_play_mode_correction:bool=True, wait_for_server:bool=True, hear_callback=None) -> None: + + self.radio = None # hear_message may be called during Server_Comm instantiation + self.logger = Logger(enable_log, f"{team_name}_{unum}") + self.world = World(robot_type, team_name, unum, apply_play_mode_correction, enable_draw, self.logger, host) + self.world_parser = World_Parser(self.world, self.hear_message if hear_callback is None else hear_callback) + self.scom = Server_Comm(host,agent_port,monitor_port,unum,robot_type,team_name,self.world_parser,self.world,Base_Agent.all_agents,wait_for_server) + self.inv_kinematics = Inverse_Kinematics(self.world.robot) + self.behavior = Behavior(self) + self.path_manager = Path_Manager(self.world) + self.radio = Radio(self.world, self.scom.commit_announcement) + self.behavior.create_behaviors() + Base_Agent.all_agents.append(self) + + @abstractmethod + def think_and_send(self): + pass + + def hear_message(self, msg:bytearray, direction, timestamp:float) -> None: + if direction != "self" and self.radio is not None: + self.radio.receive(msg) + + def terminate(self): + # close shared monitor socket if this is the last agent on this thread + self.scom.close(close_monitor_socket=(len(Base_Agent.all_agents)==1)) + Base_Agent.all_agents.remove(self) + + @staticmethod + def terminate_all(): + for o in Base_Agent.all_agents: + o.scom.close(True) # close shared monitor socket, if it exists + Base_Agent.all_agents = [] + diff --git a/behaviors/Behavior.py b/behaviors/Behavior.py new file mode 100644 index 0000000..acbb4be --- /dev/null +++ b/behaviors/Behavior.py @@ -0,0 +1,183 @@ +import numpy as np + + +class Behavior(): + + def __init__(self, base_agent) -> None: + from agent.Base_Agent import Base_Agent # for type hinting + self.base_agent : Base_Agent = base_agent + self.world = self.base_agent.world + self.state_behavior_name = None + self.state_behavior_init_ms = 0 + self.previous_behavior = None + self.previous_behavior_duration = None + + #Initialize standard behaviors + from behaviors.Poses import Poses + from behaviors.Slot_Engine import Slot_Engine + from behaviors.Head import Head + + self.poses = Poses(self.world) + self.slot_engine = Slot_Engine(self.world) + self.head = Head(self.world) + + + def create_behaviors(self): + ''' + Behaviors dictionary: + creation: key: ( description, auto_head, lambda reset[,a,b,c,..]: self.execute(...), lambda: self.is_ready(...) ) + usage: key: ( description, auto_head, execute_func(reset, *args), is_ready_func ) + ''' + self.behaviors = self.poses.get_behaviors_callbacks() + self.behaviors.update(self.slot_engine.get_behaviors_callbacks()) + self.behaviors.update(self.get_custom_callbacks()) + + + def get_custom_callbacks(self): + ''' + Searching custom behaviors could be implemented automatically + However, for code distribution, loading code dynamically is not ideal (unless we load byte code or some other import solution) + Currently, adding custom behaviors is a manual process: + 1. Add import statement below + 2. Add class to 'classes' list + ''' + + # Declaration of behaviors + from behaviors.custom.Basic_Kick.Basic_Kick import Basic_Kick + from behaviors.custom.Dribble.Dribble import Dribble + from behaviors.custom.Fall.Fall import Fall + from behaviors.custom.Get_Up.Get_Up import Get_Up + from behaviors.custom.Step.Step import Step + from behaviors.custom.Walk.Walk import Walk + classes = [Basic_Kick,Dribble,Fall,Get_Up,Step,Walk] + + '''---- End of manual declarations ----''' + + # Prepare callbacks + self.objects = {cls.__name__ : cls(self.base_agent) for cls in classes} + + return {name: (o.description,o.auto_head, + lambda reset,*args,o=o: o.execute(reset,*args), lambda *args,o=o: o.is_ready(*args)) for name, o in self.objects.items()} + + + def get_custom_behavior_object(self, name): + ''' Get unique object from class "name" ("name" must represent a custom behavior) ''' + assert name in self.objects, f"There is no custom behavior called {name}" + return self.objects[name] + + + def get_all_behaviors(self): + ''' Get name and description of all behaviors ''' + return [ key for key in self.behaviors ], [ val[0] for val in self.behaviors.values() ] + + + def get_current(self): + ''' Get name and duration (in seconds) of current behavior ''' + duration = (self.world.time_local_ms - self.state_behavior_init_ms) / 1000.0 + return self.state_behavior_name, duration + + + def get_previous(self): + ''' Get name and duration (in seconds) of previous behavior ''' + return self.previous_behavior, self.previous_behavior_duration + + + def force_reset(self): + ''' Force reset next executed behavior ''' + self.state_behavior_name = None + + + def execute(self, name, *args) -> bool: + ''' + Execute one step of behavior `name` with arguments `*args` + - Automatically resets behavior on first call + - Call get_current() to get the current behavior (and its duration) + + Returns + ------- + finished : bool + True if behavior has finished + ''' + + assert name in self.behaviors, f"Behavior {name} does not exist!" + + # Check if transitioning from other behavior + reset = bool(self.state_behavior_name != name) + if reset: + if self.state_behavior_name is not None: + self.previous_behavior = self.state_behavior_name # Previous behavior was interrupted (did not finish) + self.previous_behavior_duration = (self.world.time_local_ms - self.state_behavior_init_ms) / 1000.0 + self.state_behavior_name = name + self.state_behavior_init_ms = self.world.time_local_ms + + # Control head orientation if behavior allows it + if self.behaviors[name][1]: + self.head.execute() + + # Execute behavior + if not self.behaviors[name][2](reset,*args): + return False + + # The behavior has finished + self.previous_behavior = self.state_behavior_name # Store current behavior name + self.state_behavior_name = None + return True + + + def execute_sub_behavior(self, name, reset, *args): + ''' + Execute one step of behavior `name` with arguments `*args` + Useful for custom behaviors that call other behaviors + - Behavior reset is performed manually + - Calling get_current() will return the main behavior (not the sub behavior) + - Poses ignore the reset argument + + Returns + ------- + finished : bool + True if behavior has finished + ''' + + assert name in self.behaviors, f"Behavior {name} does not exist!" + + # Control head orientation if behavior allows it + if self.behaviors[name][1]: + self.head.execute() + + # Execute behavior + return self.behaviors[name][2](reset,*args) + + + def execute_to_completion(self, name, *args): + ''' + Execute steps and communicate with server until completion + - Slot behaviors indicate that the behavior has finished when sending the last command (which is promptly sent) + - Poses are finished when the server returns the desired robot state (so the last command is irrelevant) + - For custom behaviors, we assume the same logic, and so, the last command is ignored + + Notes + ----- + - Before exiting, the `Robot.joints_target_speed` array is reset to avoid polluting the next command + - For poses and custom behaviors that indicate a finished behavior on the 1st call, nothing is committed or sent + - Warning: this function may get stuck in an infinite loop if the behavior never ends + ''' + + r = self.world.robot + skip_last = name not in self.slot_engine.behaviors + + while True: + done = self.execute(name, *args) + if done and skip_last: break # Exit here if last command is irrelevant + self.base_agent.scom.commit_and_send( r.get_command() ) + self.base_agent.scom.receive() + if done: break # Exit here if last command is part of the behavior + + # reset to avoid polluting the next command + r.joints_target_speed = np.zeros_like(r.joints_target_speed) + + + def is_ready(self, name, *args) -> bool: + ''' Checks if behavior is ready to start under current game/robot conditions ''' + + assert name in self.behaviors, f"Behavior {name} does not exist!" + return self.behaviors[name][3](*args) \ No newline at end of file diff --git a/behaviors/Head.py b/behaviors/Head.py new file mode 100644 index 0000000..9c3383c --- /dev/null +++ b/behaviors/Head.py @@ -0,0 +1,105 @@ +from math_ops.Math_Ops import Math_Ops as M +from world.World import World +import numpy as np + +class Head(): + FIELD_FLAGS = World.FLAGS_CORNERS_POS + World.FLAGS_POSTS_POS + HEAD_PITCH = -35 + + def __init__(self, world : World) -> None: + self.world = world + self.look_left = True + self.state = 0 + + + def execute(self): + ''' + Try to compute best head orientation if possible, otherwise look around + + state: + 0 - Adjust position - ball is in FOV and robot can self-locate + 1..TIMEOUT-1 - Guided search - attempt to use recent visual/radio information to guide the search + TIMEOUT - Random search - look around (default mode after guided search fails by timeout) + ''' + TIMEOUT = 30 + w = self.world + r = w.robot + can_self_locate = r.loc_last_update > w.time_local_ms - w.VISUALSTEP_MS + + #--------------------------------------- A. Ball is in FOV and robot can self-locate + + if w.ball_last_seen > w.time_local_ms - w.VISUALSTEP_MS: # ball is in FOV + if can_self_locate: + best_dir = self.compute_best_direction(can_self_locate, use_ball_from_vision=True) + self.state = 0 + elif self.state < TIMEOUT: + #--------------------------------------- B. Ball is in FOV but robot cannot currently self-locate + best_dir = self.compute_best_direction(can_self_locate, use_ball_from_vision=True) + self.state += 1 # change to guided search and increment time + elif self.state < TIMEOUT: + #--------------------------------------- C. Ball is not in FOV + best_dir = self.compute_best_direction(can_self_locate) + self.state += 1 # change to guided search and increment time + + + if self.state == TIMEOUT: # Random search + + if w.ball_last_seen > w.time_local_ms - w.VISUALSTEP_MS: # Ball is in FOV (search 45 deg to both sides of the ball) + ball_dir = M.vector_angle(w.ball_rel_torso_cart_pos[:2]) + targ = np.clip(ball_dir + (45 if self.look_left else -45), -119, 119) + else: # Ball is not in FOV (search 119 deg to both sides) + targ = 119 if self.look_left else -119 + + if r.set_joints_target_position_direct([0,1], np.array([targ,Head.HEAD_PITCH]), False) <= 0: + self.look_left = not self.look_left + + else: # Adjust position or guided search + r.set_joints_target_position_direct([0,1], np.array([best_dir,Head.HEAD_PITCH]), False) + + + def compute_best_direction(self, can_self_locate, use_ball_from_vision=False): + FOV_MARGIN = 15 # safety margin, avoid margin horizontally + SAFE_RANGE = 120 - FOV_MARGIN*2 + HALF_RANGE = SAFE_RANGE / 2 + + w = self.world + r = w.robot + + if use_ball_from_vision: + ball_2d_dist = np.linalg.norm(w.ball_rel_torso_cart_pos[:2]) + else: + ball_2d_dist = np.linalg.norm(w.ball_abs_pos[:2]-r.loc_head_position[:2]) + + if ball_2d_dist > 0.12: + if use_ball_from_vision: + ball_dir = M.vector_angle(w.ball_rel_torso_cart_pos[:2]) + else: + ball_dir = M.target_rel_angle(r.loc_head_position, r.imu_torso_orientation, w.ball_abs_pos) + else: # ball is very close to robot + ball_dir = 0 + + flags_diff = dict() + + # iterate flags + for f in Head.FIELD_FLAGS: + flag_dir = M.target_rel_angle(r.loc_head_position, r.imu_torso_orientation, f) + diff = M.normalize_deg( flag_dir - ball_dir ) + if abs(diff) < HALF_RANGE and can_self_locate: + return ball_dir # return ball direction if robot can self-locate + flags_diff[f] = diff + + + closest_flag = min( flags_diff, key=lambda k: abs(flags_diff[k]) ) + closest_diff = flags_diff[closest_flag] + + if can_self_locate: # at this point, if it can self-locate, then abs(closest_diff) > HALF_RANGE + # return position that centers the ball as much as possible in the FOV, including the nearest flag if possible + final_diff = min( abs(closest_diff) - HALF_RANGE, SAFE_RANGE ) * np.sign(closest_diff) + else: + # position that centers the flag as much as possible, until it is seen, while keeping the ball in the FOV + final_diff = np.clip( closest_diff, -SAFE_RANGE, SAFE_RANGE ) + # saturate instead of normalizing angle to avoid a complete neck rotation + return np.clip(ball_dir + final_diff, -119, 119) + + + return M.normalize_deg( ball_dir + final_diff ) \ No newline at end of file diff --git a/behaviors/Poses.py b/behaviors/Poses.py new file mode 100644 index 0000000..80458e2 --- /dev/null +++ b/behaviors/Poses.py @@ -0,0 +1,97 @@ +''' +Pose - angles in degrees for the specified joints +Note: toes positions are ignored by robots that have no toes + +Poses may control all joints or just a subgroup defined by the "indices" variable +''' + +import numpy as np +from world.World import World + + +class Poses(): + def __init__(self, world : World) -> None: + self.world = world + self.tolerance = 0.05 # angle error tolerance to consider that behavior is finished + + ''' + Instruction to add new pose: + 1. add new entry to the following dictionary, using a unique behavior name + 2. that's it + ''' + self.poses = { + "Zero":( + "Neutral pose, including head", # description + False, # disable automatic head orientation + np.array([0,1,2,3,4,5,6,7,8,9,10,11,12,13, 14, 15,16,17,18,19,20,21,22,23]), # indices + np.array([0,0,0,0,0,0,0,0,0,0, 0, 0, 0, 0,-90,-90, 0, 0,90,90, 0, 0, 0, 0]) # values + ), + "Zero_Legacy":( + "Neutral pose, including head, elbows cause collision (legacy)", # description + False, # disable automatic head orientation + np.array([0,1,2,3,4,5,6,7,8,9,10,11,12,13, 14, 15,16,17,18,19,20,21,22,23]), # indices + np.array([0,0,0,0,0,0,0,0,0,0, 0, 0, 0, 0,-90,-90, 0, 0, 0, 0, 0, 0, 0, 0]) # values + ), + "Zero_Bent_Knees":( + "Neutral pose, including head, bent knees", # description + False, # disable automatic head orientation + np.array([0,1,2,3,4,5,6, 7, 8, 9,10,11,12,13, 14, 15,16,17,18,19,20,21,22,23]), # indices + np.array([0,0,0,0,0,0,30,30,-60,-60,30,30, 0, 0,-90,-90, 0, 0,90,90, 0, 0, 0, 0]) # values + ), + "Zero_Bent_Knees_Auto_Head":( + "Neutral pose, automatic head, bent knees", # description + True, # enable automatic head orientation + np.array([2,3,4,5,6, 7, 8, 9,10,11,12,13, 14, 15,16,17,18,19,20,21,22,23]), # indices + np.array([0,0,0,0,30,30,-60,-60,30,30, 0, 0,-90,-90, 0, 0,90,90, 0, 0, 0, 0]) # values + ), + "Fall_Back":( + "Incline feet to fall back", # description + True, # enable automatic head orientation + np.array([ 10, 11]), # indices + np.array([-20,-20]) # values + ), + "Fall_Front":( + "Incline feet to fall forward", # description + True, # enable automatic head orientation + np.array([10,11]), # indices + np.array([45,45]) # values + ), + "Fall_Left":( + "Incline legs to fall to left", # description + True, # enable automatic head orientation + np.array([ 4, 5]), # indices + np.array([-20,20]) # values + ), + "Fall_Right":( + "Incline legs to fall to right", # description + True, # enable automatic head orientation + np.array([ 4, 5]), # indices + np.array([20,-20]) # values + ), + } + + # Remove toes if not robot 4 + if world.robot.type != 4: + for key, val in self.poses.items(): + idxs = np.where(val[2] >= 22)[0] # search for joint 22 & 23 + if len(idxs) > 0: + self.poses[key] = (val[0], val[1], np.delete(val[2],idxs), np.delete(val[3],idxs)) # remove those joints + + + def get_behaviors_callbacks(self): + ''' + Returns callbacks for each pose behavior (used internally) + + Implementation note: + -------------------- + Using dummy default parameters because lambda expression will remember the scope and var name. + In the loop, the scope does not change, nor does the var name. + However, default parameters are evaluated when the lambda is defined. + ''' + return {key: (val[0], val[1], lambda reset, key=key: self.execute(key), lambda: True) for key, val in self.poses.items()} + + def execute(self,name) -> bool: + _, _, indices, values = self.poses[name] + remaining_steps = self.world.robot.set_joints_target_position_direct(indices,values,True,tolerance=self.tolerance) + return bool(remaining_steps == -1) + diff --git a/behaviors/Slot_Engine.py b/behaviors/Slot_Engine.py new file mode 100644 index 0000000..7566228 --- /dev/null +++ b/behaviors/Slot_Engine.py @@ -0,0 +1,110 @@ +from math_ops.Math_Ops import Math_Ops as M +from os import listdir +from os.path import isfile, join +from world.World import World +import numpy as np +import xml.etree.ElementTree as xmlp + +class Slot_Engine(): + + def __init__(self, world : World) -> None: + self.world = world + self.state_slot_number = 0 + self.state_slot_start_time = 0 + self.state_slot_start_angles = None + self.state_init_zero = True + + # ------------- Parse slot behaviors + + dir = M.get_active_directory("/behaviors/slot/") + + common_dir = f"{dir}common/" + files = [(f,join(common_dir, f)) for f in listdir(common_dir) if isfile(join(common_dir, f)) and f.endswith(".xml")] + robot_dir = f"{dir}r{world.robot.type}" + files += [(f,join(robot_dir, f)) for f in listdir(robot_dir) if isfile(join(robot_dir, f)) and f.endswith(".xml")] + + self.behaviors = dict() + self.descriptions = dict() + self.auto_head_flags = dict() + + for fname, file in files: + robot_xml_root = xmlp.parse(file).getroot() + slots = [] + bname = fname[:-4] # remove extension ".xml" + + for xml_slot in robot_xml_root: + assert xml_slot.tag == 'slot', f"Unexpected XML element in slot behavior {fname}: '{xml_slot.tag}'" + indices, angles = [],[] + + for action in xml_slot: + indices.append( int(action.attrib['id']) ) + angles.append( float(action.attrib['angle']) ) + + delta_ms = float(xml_slot.attrib['delta']) * 1000 + assert delta_ms > 0, f"Invalid delta <=0 found in Slot Behavior {fname}" + slots.append((delta_ms, indices, angles)) + + assert bname not in self.behaviors, f"Found at least 2 slot behaviors with same name: {fname}" + + self.descriptions[bname] = robot_xml_root.attrib["description"] if "description" in robot_xml_root.attrib else bname + self.auto_head_flags[bname] = (robot_xml_root.attrib["auto_head"] == "1") + self.behaviors[bname] = slots + + + def get_behaviors_callbacks(self): + ''' + Returns callbacks for each slot behavior (used internally) + + Implementation note: + -------------------- + Using dummy default parameters because lambda expression will remember the scope and var name. + In the loop, the scope does not change, nor does the var name. + However, default parameters are evaluated when the lambda is defined. + ''' + return {key: (self.descriptions[key],self.auto_head_flags[key], + lambda reset,key=key: self.execute(key,reset), lambda key=key: self.is_ready(key)) for key in self.behaviors} + + + def is_ready(self,name) -> bool: + return True + + + def reset(self, name): + ''' Initialize/Reset slot behavior ''' + + self.state_slot_number = 0 + self.state_slot_start_time_ms = self.world.time_local_ms + self.state_slot_start_angles = np.copy(self.world.robot.joints_position) + assert name in self.behaviors, f"Requested slot behavior does not exist: {name}" + + + def execute(self,name,reset) -> bool: + ''' Execute one step ''' + + if reset: self.reset(name) + + elapsed_ms = self.world.time_local_ms - self.state_slot_start_time_ms + delta_ms, indices, angles = self.behaviors[name][self.state_slot_number] + + # Check slot progression + if elapsed_ms >= delta_ms: + self.state_slot_start_angles[indices] = angles #update start angles based on last target + + # Prevent 2 rare scenarios: + # 1 - this function is called after the behavior is finished & reset==False + # 2 - we are in the last slot, syncmode is not active, and we lost the last step + if self.state_slot_number+1 == len(self.behaviors[name]): + return True # So, the return indicates a finished behavior until a reset is sent via the arguments + + self.state_slot_number += 1 + elapsed_ms = 0 + self.state_slot_start_time_ms = self.world.time_local_ms + delta_ms, indices, angles = self.behaviors[name][self.state_slot_number] + + # Execute + progress = (elapsed_ms+20) / delta_ms + target = (angles - self.state_slot_start_angles[indices]) * progress + self.state_slot_start_angles[indices] + self.world.robot.set_joints_target_position_direct(indices,target,False) + + # Return True if finished (this is the last step) + return bool(elapsed_ms+20 >= delta_ms and self.state_slot_number + 1 == len(self.behaviors[name])) # true if next step (now+20ms) is out of bounds diff --git a/behaviors/custom/Basic_Kick/Basic_Kick.py b/behaviors/custom/Basic_Kick/Basic_Kick.py new file mode 100644 index 0000000..274bbb8 --- /dev/null +++ b/behaviors/custom/Basic_Kick/Basic_Kick.py @@ -0,0 +1,74 @@ +from agent.Base_Agent import Base_Agent +from behaviors.custom.Step.Step_Generator import Step_Generator +from math_ops.Math_Ops import Math_Ops as M + + +class Basic_Kick(): + + def __init__(self, base_agent : Base_Agent) -> None: + self.behavior = base_agent.behavior + self.path_manager = base_agent.path_manager + self.world = base_agent.world + self.description = "Walk to ball and perform a basic kick" + self.auto_head = True + + r_type = self.world.robot.type + self.bias_dir = [22,29,26,29,22][self.world.robot.type] + self.ball_x_limits = ((0.19,0.215), (0.2,0.22), (0.19,0.22), (0.2,0.215), (0.2,0.215))[r_type] + self.ball_y_limits = ((-0.115,-0.1), (-0.125,-0.095), (-0.12,-0.1), (-0.13,-0.105), (-0.09,-0.06))[r_type] + self.ball_x_center = (self.ball_x_limits[0] + self.ball_x_limits[1])/2 + self.ball_y_center = (self.ball_y_limits[0] + self.ball_y_limits[1])/2 + + def execute(self,reset, direction, abort=False) -> bool: # You can add more arguments + ''' + Parameters + ---------- + direction : float + kick direction relative to field, in degrees + abort : bool + True to abort. + The method returns True upon successful abortion, which is immediate while the robot is aligning itself. + However, if the abortion is requested during the kick, it is delayed until the kick is completed. + ''' + + w = self.world + r = self.world.robot + b = w.ball_rel_torso_cart_pos + t = w.time_local_ms + gait : Step_Generator = self.behavior.get_custom_behavior_object("Walk").env.step_generator + + if reset: + self.phase = 0 + self.reset_time = t + + if self.phase == 0: + biased_dir = M.normalize_deg(direction + self.bias_dir) # add bias to rectify direction + ang_diff = abs(M.normalize_deg( biased_dir - r.loc_torso_orientation )) # the reset was learned with loc, not IMU + + next_pos, next_ori, dist_to_final_target = self.path_manager.get_path_to_ball( + x_ori=biased_dir, x_dev=-self.ball_x_center, y_dev=-self.ball_y_center, torso_ori=biased_dir) + + if (w.ball_last_seen > t - w.VISUALSTEP_MS and ang_diff < 5 and # ball is visible & aligned + self.ball_x_limits[0] < b[0] < self.ball_x_limits[1] and # ball is in kick area (x) + self.ball_y_limits[0] < b[1] < self.ball_y_limits[1] and # ball is in kick area (y) + t - w.ball_abs_pos_last_update < 100 and # ball absolute location is recent + dist_to_final_target < 0.03 and # if absolute ball position is updated + not gait.state_is_left_active and gait.state_current_ts == 2 and # walk gait phase is adequate + t - self.reset_time > 500): # to avoid kicking immediately without preparation & stability + + self.phase += 1 + + return self.behavior.execute_sub_behavior("Kick_Motion", True) + else: + dist = max(0.07, dist_to_final_target) + reset_walk = reset and self.behavior.previous_behavior != "Walk" # reset walk if it wasn't the previous behavior + self.behavior.execute_sub_behavior("Walk", reset_walk, next_pos, True, next_ori, True, dist) # target, is_target_abs, ori, is_ori_abs, distance + return abort # abort only if self.phase == 0 + + else: # define kick parameters and execute + return self.behavior.execute_sub_behavior("Kick_Motion", False) + + + def is_ready(self) -> any: # You can add more arguments + ''' Returns True if this behavior is ready to start/continue under current game/robot conditions ''' + return True diff --git a/behaviors/custom/Dribble/Dribble.py b/behaviors/custom/Dribble/Dribble.py new file mode 100644 index 0000000..d5518b3 --- /dev/null +++ b/behaviors/custom/Dribble/Dribble.py @@ -0,0 +1,207 @@ +from agent.Base_Agent import Base_Agent +from behaviors.custom.Dribble.Env import Env +from math_ops.Math_Ops import Math_Ops as M +from math_ops.Neural_Network import run_mlp +import numpy as np +import pickle + + +class Dribble(): + + def __init__(self, base_agent : Base_Agent) -> None: + self.behavior = base_agent.behavior + self.path_manager = base_agent.path_manager + self.world = base_agent.world + self.description = "RL dribble" + self.auto_head = True + self.env = Env(base_agent, 0.9 if self.world.robot.type == 3 else 1.2) + + with open(M.get_active_directory([ + "/behaviors/custom/Dribble/dribble_R0.pkl", + "/behaviors/custom/Dribble/dribble_R1.pkl", + "/behaviors/custom/Dribble/dribble_R2.pkl", + "/behaviors/custom/Dribble/dribble_R3.pkl", + "/behaviors/custom/Dribble/dribble_R4.pkl" + ][self.world.robot.type]), 'rb') as f: + self.model = pickle.load(f) + + def define_approach_orientation(self): + + w = self.world + b = w.ball_abs_pos[:2] + me = w.robot.loc_head_position[:2] + + self.approach_orientation = None + + MARGIN = 0.8 # safety margin (if ball is near the field limits by this amount, the approach orientation is considered) + M90 = 90/MARGIN # auxiliary variable + DEV = 25 # when standing on top of sidelines or endlines, the approach direction deviates from that line by this amount + MDEV = (90+DEV)/MARGIN # auxiliary variable + + a1 = -180 # angle range start (counterclockwise rotation) + a2 = 180 # angle range end (counterclockwise rotation) + + if b[1] < -10 + MARGIN: + if b[0] < -15 + MARGIN: + a1 = DEV - M90 * (b[1]+10) + a2 = 90 - DEV + M90 * (b[0]+15) + elif b[0] > 15 - MARGIN: + a1 = 90 + DEV - M90 * (15-b[0]) + a2 = 180 - DEV + M90 * (b[1]+10) + else: + a1 = DEV - MDEV * (b[1]+10) + a2 = 180 - DEV + MDEV * (b[1]+10) + elif b[1] > 10 - MARGIN: + if b[0] < -15 + MARGIN: + a1 = -90 + DEV - M90 * (b[0]+15) + a2 = -DEV + M90 * (10-b[1]) + elif b[0] > 15 - MARGIN: + a1 = 180 + DEV - M90 * (10-b[1]) + a2 = 270 - DEV + M90 * (15-b[0]) + else: + a1 = -180 + DEV - MDEV * (10-b[1]) + a2 = -DEV + MDEV * (10-b[1]) + elif b[0] < -15 + MARGIN: + a1 = -90 + DEV - MDEV * (b[0]+15) + a2 = 90 - DEV + MDEV * (b[0]+15) + elif b[0] > 15 - MARGIN and abs(b[1]) > 1.2: + a1 = 90 + DEV - MDEV * (15-b[0]) + a2 = 270 - DEV + MDEV * (15-b[0]) + + + cad = M.vector_angle(b - me) # current approach direction + + a1 = M.normalize_deg(a1) + a2 = M.normalize_deg(a2) + + if a10.4 + + if reset: + self.phase = 0 + if behavior.previous_behavior == "Push_RL" and 0 0.4: # stop defining orientation after getting near the ball to avoid noise + self.define_approach_orientation() + + #------------------------ 2A. A better approach orientation is needed (ball is almost out of bounds) + if self.approach_orientation is not None: + next_pos, next_ori, dist_to_final_target = self.path_manager.get_path_to_ball( + x_ori=self.approach_orientation, x_dev=-0.24, torso_ori=self.approach_orientation, safety_margin=0.4) + + if b_rel[0]<0.26 and b_rel[0]>0.18 and abs(b_rel[1])<0.04 and w.ball_is_visible: # ready to start dribble + self.phase += 1 + reset_dribble = True + else: + dist = max(0.08, dist_to_final_target*0.7) + behavior.execute_sub_behavior("Walk", reset_walk, next_pos, True, next_ori, True, dist) # target, is_target_abs, ori, is_ori_abs, distance + + #------------------------ 2B. A better approach orientation is not needed but the robot cannot see the ball + elif w.time_local_ms - w.ball_last_seen > 200: # walk to absolute target if ball was not seen + abs_ori = M.vector_angle( b - me ) + behavior.execute_sub_behavior("Walk", reset_walk, b, True, abs_ori, True, None) # target, is_target_abs, ori, is_ori_abs, distance + + #------------------------ 2C. A better approach orientation is not needed and the robot can see the ball + else: # walk to relative target + if 0.18 0.5 and lost_ball)): # go back to walking + self.phase += 1 + elif self.phase == 1: # dribble + #------------------------ 1. Define dribble parameters + self.env.dribble_speed = speed + + # Relative orientation values are decreased to avoid overshoot + if orientation is None: + if b[0] < 0: # dribble to the sides + if b[1] > 0: + dribble_target = (15,5) + else: + dribble_target = (15,-5) + else: + dribble_target = None # go to goal + self.env.dribble_rel_orientation = self.path_manager.get_dribble_path(optional_2d_target=dribble_target)[1] + elif is_orientation_absolute: + self.env.dribble_rel_orientation = M.normalize_deg( orientation - r.imu_torso_orientation ) + else: + self.env.dribble_rel_orientation = float(orientation) # copy if numpy float + + #------------------------ 2. Execute behavior + obs = self.env.observe(reset_dribble) + action = run_mlp(obs, self.model) + self.env.execute(action) + + # wind down dribbling, and then reset phase + if self.phase > 1: + WIND_DOWN_STEPS = 60 + #------------------------ 1. Define dribble wind down parameters + self.env.dribble_speed = 1 - self.phase/WIND_DOWN_STEPS + self.env.dribble_rel_orientation = 0 + + #------------------------ 2. Execute behavior + obs = self.env.observe(reset_dribble, virtual_ball=True) + action = run_mlp(obs, self.model) + self.env.execute(action) + + #------------------------ 3. Reset behavior + self.phase += 1 + if self.phase >= WIND_DOWN_STEPS - 5: + self.phase = 0 + return True + + + return False + + def is_ready(self): + ''' Returns True if this behavior is ready to start/continue under current game/robot conditions ''' + return True \ No newline at end of file diff --git a/behaviors/custom/Dribble/Env.py b/behaviors/custom/Dribble/Env.py new file mode 100644 index 0000000..479e17d --- /dev/null +++ b/behaviors/custom/Dribble/Env.py @@ -0,0 +1,181 @@ +from agent.Base_Agent import Base_Agent +from behaviors.custom.Step.Step_Generator import Step_Generator +from math_ops.Math_Ops import Math_Ops as M +import math +import numpy as np + +class Env(): + def __init__(self, base_agent : Base_Agent, step_width) -> None: + + self.world = base_agent.world + self.ik = base_agent.inv_kinematics + + # State space + self.obs = np.zeros(76, np.float32) + + # Step behavior defaults + self.STEP_DUR = 8 + self.STEP_Z_SPAN = 0.02 + self.STEP_Z_MAX = 0.70 + + # IK + r = self.world.robot + nao_specs = self.ik.NAO_SPECS + self.leg_length = nao_specs[1] + nao_specs[3] # upper leg height + lower leg height + feet_y_dev = nao_specs[0] * step_width # wider step + sample_time = r.STEPTIME + max_ankle_z = nao_specs[5] + + self.step_generator = Step_Generator(feet_y_dev, sample_time, max_ankle_z) + self.DEFAULT_ARMS = np.array([-90,-90,8,8,90,90,70,70],np.float32) + + self.dribble_rel_orientation = None # relative to imu_torso_orientation (in degrees) + self.dribble_speed = 1 + + + def observe(self, init=False, virtual_ball=False): + + w = self.world + r = self.world.robot + + if init: # reset variables + self.step_counter = 0 + self.act = np.zeros(16, np.float32) # memory variable + + # index observation naive normalization + self.obs[0] = min(self.step_counter,12*8) /100 # simple counter: 0,1,2,3... + self.obs[1] = r.loc_head_z *3 # z coordinate (torso) + self.obs[2] = r.loc_head_z_vel /2 # z velocity (torso) + self.obs[3] = r.imu_torso_roll /15 # absolute torso roll in deg + self.obs[4] = r.imu_torso_pitch /15 # absolute torso pitch in deg + self.obs[5:8] = r.gyro /100 # gyroscope + self.obs[8:11] = r.acc /10 # accelerometer + + self.obs[11:17] = r.frp.get('lf', np.zeros(6)) * (10,10,10,0.01,0.01,0.01) # left foot: relative point of origin (p) and force vector (f) -> (px,py,pz,fx,fy,fz)* + self.obs[17:23] = r.frp.get('rf', np.zeros(6)) * (10,10,10,0.01,0.01,0.01) # right foot: relative point of origin (p) and force vector (f) -> (px,py,pz,fx,fy,fz)* + # *if foot is not touching the ground, then (px=0,py=0,pz=0,fx=0,fy=0,fz=0) + + self.obs[23:43] = r.joints_position[2:22] /100 # position of all joints except head & toes (for robot type 4) + self.obs[43:63] = r.joints_speed[2:22] /6.1395 # speed of all joints except head & toes (for robot type 4) + + ''' + Expected observations for walking state: + Time step R 0 1 2 3 4 5 6 7 0 + Progress 1 0 .14 .28 .43 .57 .71 .86 1 0 + Left leg active T F F F F F F F F T + ''' + + if init: # the walking parameters refer to the last parameters in effect (after a reset, they are pointless) + self.obs[63] = 1 # step progress + self.obs[64] = 1 # 1 if left leg is active + self.obs[65] = 0 # 1 if right leg is active + self.obs[66] = 0 + else: + self.obs[63] = self.step_generator.external_progress # step progress + self.obs[64] = float(self.step_generator.state_is_left_active) # 1 if left leg is active + self.obs[65] = float(not self.step_generator.state_is_left_active) # 1 if right leg is active + self.obs[66] = math.sin(self.step_generator.state_current_ts / self.step_generator.ts_per_step * math.pi) + + # Ball + ball_rel_hip_center = self.ik.torso_to_hip_transform(w.ball_rel_torso_cart_pos) + ball_dist_hip_center = np.linalg.norm( ball_rel_hip_center ) + + if init: + self.obs[67:70] = (0,0,0) # Initial velocity is 0 + elif w.ball_is_visible: + self.obs[67:70] = (ball_rel_hip_center - self.obs[70:73]) * 10 # Ball velocity, relative to ankle's midpoint + + self.obs[70:73] = ball_rel_hip_center # Ball position, relative to hip + self.obs[73] = ball_dist_hip_center * 2 + + if virtual_ball: # simulate the ball between the robot's feet + self.obs[67:74] = (0,0,0,0.05,0,-0.175,0.36) + + ''' + Create internal target with a smoother variation + ''' + + MAX_ROTATION_DIFF = 20 # max difference (degrees) per visual step + MAX_ROTATION_DIST = 80 + + if init: + self.internal_rel_orientation = 0 + self.internal_target_vel = 0 + self.gym_last_internal_abs_ori = r.imu_torso_orientation # for training purposes (reward) + + + #---------------------------------------------------------------- compute internal target + + if w.vision_is_up_to_date: + + previous_internal_rel_orientation = np.copy(self.internal_rel_orientation) + + internal_ori_diff = np.clip( M.normalize_deg( self.dribble_rel_orientation - self.internal_rel_orientation ), -MAX_ROTATION_DIFF, MAX_ROTATION_DIFF) + self.internal_rel_orientation = np.clip(M.normalize_deg( self.internal_rel_orientation + internal_ori_diff ), -MAX_ROTATION_DIST, MAX_ROTATION_DIST) + + # Observations + self.internal_target_vel = self.internal_rel_orientation - previous_internal_rel_orientation + + self.gym_last_internal_abs_ori = self.internal_rel_orientation + r.imu_torso_orientation + + #----------------------------------------------------------------- observations + + self.obs[74] = self.internal_rel_orientation / MAX_ROTATION_DIST + self.obs[75] = self.internal_target_vel / MAX_ROTATION_DIFF + + return self.obs + + + def execute_ik(self, l_pos, l_rot, r_pos, r_rot): + r = self.world.robot + # Apply IK to each leg + Set joint targets + + # Left leg + indices, self.values_l, error_codes = self.ik.leg(l_pos, l_rot, True, dynamic_pose=False) + + r.set_joints_target_position_direct(indices, self.values_l, harmonize=False) + + # Right leg + indices, self.values_r, error_codes = self.ik.leg(r_pos, r_rot, False, dynamic_pose=False) + + r.set_joints_target_position_direct(indices, self.values_r, harmonize=False) + + + def execute(self, action): + + r = self.world.robot + + # Actions: + # 0,1,2 left ankle pos + # 3,4,5 right ankle pos + # 6,7,8 left foot rotation + # 9,10,11 right foot rotation + # 12,13 left/right arm pitch + # 14,15 left/right arm roll + + # exponential moving average + self.act = 0.85 * self.act + 0.15 * action * 0.7 * 0.95 * self.dribble_speed + + # execute Step behavior to extract the target positions of each leg (we will override these targets) + lfy,lfz,rfy,rfz = self.step_generator.get_target_positions(self.step_counter == 0, self.STEP_DUR, self.STEP_Z_SPAN, self.leg_length * self.STEP_Z_MAX) + + # Leg IK + a = self.act + l_ankle_pos = (a[0]*0.025-0.01, a[1]*0.01 + lfy, a[2]*0.01 + lfz) + r_ankle_pos = (a[3]*0.025-0.01, a[4]*0.01 + rfy, a[5]*0.01 + rfz) + l_foot_rot = a[6:9] * (2,2,3) + r_foot_rot = a[9:12] * (2,2,3) + + # Limit leg yaw/pitch (and add bias) + l_foot_rot[2] = max(0,l_foot_rot[2] + 18.3) + r_foot_rot[2] = min(0,r_foot_rot[2] - 18.3) + + # Arms actions + arms = np.copy(self.DEFAULT_ARMS) # default arms pose + arms[0:4] += a[12:16]*4 # arms pitch+roll + + # Set target positions + self.execute_ik(l_ankle_pos, l_foot_rot, r_ankle_pos, r_foot_rot) # legs + r.set_joints_target_position_direct( slice(14,22), arms, harmonize=False ) # arms + + self.step_counter += 1 \ No newline at end of file diff --git a/behaviors/custom/Dribble/dribble_R0.pkl b/behaviors/custom/Dribble/dribble_R0.pkl new file mode 100644 index 0000000000000000000000000000000000000000..7d0e0502c4e0d761dca4c566258e9043879a0efa GIT binary patch literal 24148 zcmX_ncUVt<_`=1#_5FUY>vyhmoqx{j{PVnC=f3Xex$iq#CU<&(#Q$E4a>fqaX+y&{gs$6S z5xjQ&Qj5?HYa&-JS-*bCmYlS~f$NtBuMLZcT)!bWGG|Xtn(Y5B%t@0E3;Dk#dvaz- zB;-t%49ZE&@y;1LLvjWtEO5Gcjr(KbN_n?ukiM#(pzBJEKzXwUm3f`y z57qCYNgl#FmqP~y=PyZ9mm)7X7ZB>ac1nrhRbT+g=lGHBEkhdV`jEn+G6jAvv0&bu z=A38zjB{Lmk{kVRC+O`e-aTOnucJL2{{FM)at11~ zLS}{h{}gnE;{Pw_43AwoE7S){pdKT5^Y4sIzCgcj|A*8@?{_1IEbfAeC3p7U4H&UKiAj&j#MmBbu!R&C((9e1MA-w0I%XzSe4OH3>|05M8#9-@#_Pu zXUa-=TR(;d+ljadU9#viqJYL!Jp(-qqm^(23ZrYC-)+`qgSSkE#_AAQy-ppHrwAcY z--;b9^uXNAI$rYI99B>xLzbgsS=}WK&Z)nFyKzHFG&RA1ztu7ygPTu86#d_?(%1fzmGw?L$F!wSQ{-+=wGf@r`dM}~u4SjCS z1qWu@x(gbvG+=IYx2SFHBnCqVi7n=;u*Eyl@LTvqoO;5D&6gV{F3ul>W(7I$R!2wl z>ERZ3Eu>>x zkeOcGkI@Tf;_v5IMZIaycwJX-PGXxbhQ#ea=V{YWGJ6{GP7MC_*1-q^DR#^7G5;`N zG_!P>!3Z;@Tz zc%LmX3x;JS&#A;hmOM{uu{W25XfWCy6J@jExS9p)x#rG%ZImJQ?i_psuw=dH`44@(WMGN2%2Qe93NeI>iT)m~ zfb~rpka?t#_Vx_JU)f2tHA<32_Jw20rhSb2Aq#6CNW&NHvoLRS8l>*h5^prAgaq3s zH2c{EcF#pqJiMm}<uNg2^qbMlPNMXAp>R9=LgRJ^N4=XNwO~!wM z;mV9_s2~}S7H&K|;TO^Rp}*+I{dBVStbra&b#_uA4z#~6<&RJ6=5CCLt-H3hiEj;3 z;S}PvS>#TsfitfFLq=slsL@AmmeqS0e(fQ*=Gj|V+4qY(^w*UQvP}oc%Y$&^BrRM& zc?T->&1cV|6QHrf1{~i%g++fhQLNP&`o8BbbX{DJVTt3=zI_=ybUX=r9%`_yyDah7 zJx#hF>O-IS$@I=#hSYS_vGHUrb|?f*pOxXYB{mUDA^o$udzaHU0vM&K}~$^Pc=kKJ%O4f z6Gh3vVRZJJC$reF8$FE7E?YAX5Gd7vW#BW`pZ0(rJIjbFQ`5m<@=DHcN;GdcqmnhM zo&@Wf)vTr=hx160!f{cbV0&0J|5@IOs?}4dys;b3Cz+w{0X1B;b{1dcF9J=uIhd&8 zK&I6??1TA4au(*Gnwvd7Q0k|5Zw>MK=r63j-xaMSZ&U8-P4py!IN|p!=beoL+O^{& zD|PS2>cejAV{IMZH9H>u`zz0WUOWWF`UPg~y+!r`q{;d#Rnj;uCq7=qGDC5IdL{W924AW44#r@KBgqCeOp!X~p zKL5#v!%nI!IHwI;zBN$$mWjB#`T_qSwVcgfuFet-|1m$iOtDMQUJ9IPPM$8GS;eRn zxHokvlbWCaO14+v(7HezHAV*tde3urX4|miE5>4sr2%Eh$m6ruCe*!V59$=`qnOSk z*t6y#^VNHWUCmCQ_iG{KpBc=SYOcbW$Hrsj*Z1f+PZ}*J9)~h<6PtDP1YUe9K||LV zP|(gDn4ecJnq20@&cDpyifrz}s?Uu~=KByF_B4=NsdtZhLv3lIxgvMHD-xCl%!IP- z+u+5-QSe`T1TMK*EQoM^20x`UsHyKA85>{V23;*9%gG7sn^zlI1SaE_*cxa_SV*4n zBPe}o7IE550?mJFSZ3f!bC#G<&B&qj^^iIp^wJPdj;e*6HC=p5_dNV(*d%=hE6<%w>_0Kfo2lnqwcRCx=c@%>T z&d~ADQ1I9v4(A@d;QBHTK&){tNM8zto{V+STRM+Yx2|XFO0{Xr;WT>nbs?;J{tCo~u^zY;zq2`?P?Cs-!b+c?H;#RX_mUP=jTse-!Kl)7Bm39U8l@oGqPqfLp zS&?lrjRl($IXo_?W#+pR;dkU3Hb>Wu?b^SAYD4n4rjBg(^I;cEcM9Yz@?%639Hy~R z3Uz$Q)rD;DiB)W9M;H#D8i0W|N_avTj$5r?^S5)ClmCCK;L^)kc-QNVQ(ejt!OvH- zaQz)<}7fHV}&c*#lmOR@8_`gr%BiA>{bGk8p?g{NL_ z@O6hfxhIy8pVv%UXWGgo6)^bJqlK@OmgDTA7z}u{iKTgk6H}297ujpFO39y)?bgnG zoz=-)HI^Q`{&qe&E)qR026H=hInti+3~p>hD%CGe5!`v61V0o;gY~utY~#meY)5S* z-=0;&SKbTZMm@-*yUT8K`B~Gz@x>j^I60i_YS$FUY+cH3&(gy)v#Y@|o5B7_Vg+*^ zv6|iw)aPP?-7nfnr8Av5?#-p&+H*LqT~V0(XE;XORc3`=D@0cf=ki@w*KjKWH2B2n zGvTSpZqoR94C-M#Mhmqt_@c77v^tjtO?P9tO^M=ytgZYB*SB1i_fPn}^d+)GdN{Z` zlkS)Y(MEG0_VwxuusV+7LA#ZJY1}5wMO#3y(4Wm47eJXalbCwQTfU(BKFbi^V9~Mb z(4`{_$9`FejhVTayx5qHx-Z8L2Hc`Um(}ovCgijTgY!Gc!P|(vXJsZs&Tn8gWoqIm4)CzD7fdGW@&vtd+)8tM-h1 zzlT!Rq-E@?wG_lic(dJuB%yL~3|l;5^8k)~5(NgGg42_3!+Itu-gNX2OtUv<{qr;N zNb7hmyLA(c?M;HggC$^Z@C7bSbe0`bjOJymJSk^qI==RKfJ+VLva$_XR4IK3pZ(Fo z|7z~S$--#bdMyQ}J{gN%%ks(DB%X4c3-JDnVZubCXr{uy#EIISpthNZ$iMF)!J`0e zwsyn4h^x**AQx9MoM!X-Hn`GNeHx+<>1ysmKc)I2qhC1aqhoVu&Dkt z9_~%0Ju^07%LyW_L8Ix^?gA9-cSbj}XI#RjNi6uD47Rj5u*PUryl_B5Y-}(U@+>5= z+-VPPYfz;0E#rm%y-*N8?^nUODmHN9@ftX75y6~ug)Ayp8>J!!^F@-GeB|iCFe2)% z$mZfSNOf?+t)c1s6DtD<8`3QJ5n6)7HF^YB{@@y>`DXBqKEUR_ z5n#dAgP>VB8TF%+VeG+|l(<=*X+^HXY-Vd4 zYPc^lo46+p(|JvEDbicC2QLN=Vigr5aXwkohnS1l_tT8YNZYbQapPFAT?RYDPVm*0 z7s>R9g!n^zJL?&h26y$4-)*UhaxG0G*sY1H*-H zM29D?;V&uOcK$qZn5b)!3UhcTM#(kun3*~a9h+1k@bwxd)K8+wB|Ts6Vg0n0e>z_+6^7U}|o^k9@g*2C~-pV4lIk>LzBdnM`QaC$$EweW8!4G;x z@Vx@qtA9VicbfwCKJtVa>$Ta?1?NdS?Hro_tixFw%V_srJ0`U3ry(DcoX4HBM>ns< z+{+h=Z1B})b|*rUn)_Z-+QU`Yw|o^#-noL4`_RliG>+w3pIqbCOs%KqdsFduO$01{ z>n-~A@eawLEsUEyi*EnurazIhK|{D6RGtln$3q7=cvcHkskK1h*%A29`2j5ZGKW`s zF}m*O=2EVt`2ZJbH5|^{Ph>&P#W-!^Ydm}EzAY0cf~?}$RE+N>=(4S^#E?tjVIp(H<8({MMUdt;g^;z(++Ed4!8C06MhNw~=hBVk65_MNzT?#XKvEfbo;^y)=ETj~=$N6&+WbA)oy`(B zs&5=v*gm8H)W@1>5!9yUK_8YW!=EAV1P(6`vIAKML>GTAq4(T;T5vNQN4lF}-O8oh z>xv`Xvea|jpra$WvxBPG_jU2$pcEqrzZ<|;m71`fr&r^M;xf_nk@I-9U6YyjsOj8Y z^PSumXLDSsdxC!b3B)z4*O2aqt+@T@SfINj;FVq+=bli@)%nf9jSre}HrvXo7wl#` z7Do3e$^rJ(9uVS&x2XD#xU{wxI|{Lxrzz&d5xEGj-M7{c)Z2fmO@2ER;9SXwU&%M+#^%wTa2Edr6Q_g4XgJJZ7B+C3D5?$@7 zfe$@myt-IgyfoSre!L%!`K9%gK6VR}Oqfd(zN^r+joJj>Lul00r?7Y3cZjl4W0O-{ z_*+{nv2R}xCT~u|MSk8S|FsdcijOm;m`8kDp)~SZNwj-g0(M^HShl`bUC1YEzD&|k zlu$c|R0EFl!)6AeQvFmi2+yL(bvbOs0)oh&Wh`dubMSth3WF|JF!M2yylTJ)E=g+@ z6udBHJAN)=e(6v7^Z(6dvmNJScvvxm4R2uH$8l58R@s-n01Rq*dm;|r3FaE|m# zcs2DsmXv4WJ9&LP`Je8^&vPV4!vl~*UD%;oi%QyRpc+-<~Mop+r6 z+0~f3l*fkgxp>(1tgv{}C-8ldNn0)U(@@hrvUDI`;$S~lRBF@GR$j2Cfi|cz-|PTzz(;YI7{X}w4Ttx zl&DeoEzXB!m+yclDQ{qd%mHRtQN>A)HD-V2G&8yV2l3XqPY`iuH$T_8mbdR7%}z~= z!Twp{IJD&_l~y>C|BzSsFE)!M9Q?p9(LKQ%6)xsqs-*DazUE`mj>VWZCYz<7H)JI* zhI0*F6X~*h7GntK>4jpbg!cxrJJdqVfP4nxIeRv~QuxdhRP_*N=kyb_1H zjTZW@2qEho^GW+DNPyR>rLE#b#=BS zI)Ij}_eZIW0WLl3A6J>#N^hU@pi+{EVQ~PygQjBFduJ~Bh%-j-n8@t1x6u6)!^92~ zVo;%~#u-)$;gRYwepasrlk3&uk6KGZ-uJ(FX=wnZ&-CD1wogGlby*lGJjKi0I>;Y= znMuubHzZ0lt=t{j0QJ07!Haxv&&P`pk4HoT|L^SEAxCP1R_VFe2Rk%m7Y!XJ))j&Z<9V!1Cjq`qf zgUnasXzPPCvd>)#y<8)mmy{FK_aEU36s-8|U6q1sYc*Ivh7-%HK8}yy-sV?VC!&`T zPX;gjS=WweI+>t=b|s4J<(9G7lCz25pJ_^akA^||e`y%leVtFu(&j$bqzQ78>o}#` zDj+)gmxaE#%G*tFf}IFl`@I4(-tZj9sH!ltHdo}hgLr>>g~;D-G?YL2LXn&NA>y$; zJ(4H@xq_P%Yu^e(cDLa^S2cLyz6D*<{{!5m55aYJL>vAMq3gds3WQDhsCjMyjMmd( z>z{3BoeJZy%3d9RwW*=*K1+1&X(orxFhN`3CYp8oD$Ofx9MA~n!<)mpaGRUL4*QqU zr~Sz=uKOyDks3i;SKfDENR;V#3&EXUb$6})Q1^5+zogLhDj@8IdM1H)KSnIbHyE;vaSYo5|Z zBilPE=U_g!`dbB?CBG1!EBwcQ^peKwvm5BkoDkkm>k~X&mO`UeAI7x(`FwDJ19!am z6JND8k6rYNL=C^Cu-hpG78xf&adtGwyuN~)d)MOrANv@yP{mQB$717VWj1AV4eNI^ zpao&d(5}!7K}Qadww(tXKl}h3xZXqW3tOo^rW_s_t5dUhJXKEeCYPUkXu~^s=yeN) z0BIqt@n1zZWh?R6dwro+!quTKxB)51X6;l)WH^ z-mgr9nDP5iaQGrS>-C$;uD;+!QTpujw2!PMcqg4WT|lv3iemSDA84y>A!=A`#FAFT z-uBJ>Wt$1$SAUX)=}6$G)-1d)8qWRKm5%Z+b>O{0A$}g=%ie3wf;_vKV7Ts#V9L2# zQfP32+ez(Q{F}w_MUcTc#{b7V{Win7B4bQQwr4UfGGuE#o~Ey;7?R4&U+S#mgu@+>#Q_P$S^|n=|iBq)*5$x zlwmI6W>DXfizA=v;Bl1}-d&a8!@6@MIyRa_^+%nz70Qw9q9RsfETGLb@8I*P2GqHA zpXr$D)3w2)==LE?GCIAC0;|50-kzIGa@0F+>BwogKvcv_%o@tfe-~oo$mv|v`oaOu zJ(Q;O#xUmD~>q1-|@So6+#g25^hm zh#9&B@B@QL8muQo{#2MCmy74vkqP{_4tdg2jj`EyLYBxW0L6>MxLM>))dI%~jvPoe=4ioC>i@Pk5P8LeD zIO`g2(bq(Fy)B5_T4_O=?g0Z_)de~Vty!6B0ZY>`KwU*8c6X2tyRWjBZTK|;<80*d zgv|mLI!=wb>5OC*8lPF#_(6EIZ!X@I8N~9Eb+PSfCMcx~@yYgFrgbrf#`)-C?xub8 zZFME8$Isz*-tuQSOeE?$8V|=~&+)1clDWhQCD5H2$f~AX=H?z*2Bz2NQ%%}D);ho` zUE3DZobSQ3zCE5)H&np$aZkXx$(;pUuD~Hzd^y*G;}E>)I{HU;!L3d9Xm69lCXFlS z-c6`y7Pd#&PKmFA1sAgDdf{mCqzGv^c>XmPH{d<^aQZ*kxK@DAo!r?0NpI-8o<`5l zZ(}Dn?!*sHkJ&Aa!?esJd?)10YA9(j0<#e$Ph4#d_*eqWLa^!n((7Xy=a~47BW}p?<$DT|B7W%NzX{bFpb)V z^k9am9$jCh#fsjU@a;2wsr=}3_9Co-NsPO|l9tG^$J=$8^(^2odu4K>Ka=Qhp()YH zI)UP#s~~b5iyNL5(YH4q6gIzB^xOPDXkNC9amTMhPl676yrLR*WPhV~Q}Qsb_7!`? z%c6quU}4phpA;NXN~x*C$y?AzZI^F>(5Dt3UW{jVx{OKw<#F|d0lc+t;zMq;vDp&KF?(-x(H8v0rhnF#ps7aWjVZyFe#?ZCft>7@A&kg_4^YD0KNH{vRF#_Pjn!)B_q{zhs|>zMUwOBSN2 z!wEs8#|ar&FWF9B4q|98n?^qt9O4#_ ziN^jPSGaP8TI!9JLfhO4czOO=F4uTB_gKe+TlLC_3wb{bZPZjDvo{?(MI4J>J{fcR zN732TQ#8zG1b2*UgT5=P;cebvrZzSSJbiq~H@=?oeh+8Gsf%%R?VB@gF|w{gZrnmzVhWzP!x#cmsP>L`bdZ{l%RTfbt*b{ zpC;@Zjfoy_IJ*bEB)4u4NSOa7&BAO*IeT7UdnbhgZ!d)hsr%UMsdh9YOP1VnoGD*C zpDs5n!a;7X*e<(^TrG9jvg)x^)o_BAPe{hwxr?ww*iO4orSe|BvFv2d3Gke0gmyz1 zy+4!73vOoPde092?DyG}`XiJ3t}o_fEQg?;N-J+uQN)*f4`y#}nqlG#WX~TPitU;u zg$c(O3nyiL<+k7Z1Ulvh;x!GSEUPjXUNxPj%*MlP@UIy5Z{9Gm45hf`hC=r-cIQf=+10qi)_DhOBWvGTuc_hwZc-gS`f*u z7!T%pCK%D~|D^C*lomYEieS4x@5bqx9xOz2D2n}dF_H|5)Qe#+-uk)ob6 zJ5g&2Sr0Cd@X=)U$2p#PY&k}LZUwAoR|O_6m?sPzcY?cPHv-awQ}N=TYxqIUk>nI} z>SPx8^8q2R*y;6e_;rgI+b^XKN-Yn_Ak-6{HYvcM0t1?DX9Y)H{#@#;tA@=wMNC4Y z(Yd5o9uAC=0-Gzh@t2Y>{75@5c=Pfr1^s+SFD~mbl+9({D+aOhU|nHX=2v!l@^F0a zWsBM8by<*>1Ita%hWyj_Y3~#drX?Q-6VJHQ+F(s8S`~*m(?$rMc6o4x6*c^p@I1j; z?PxFw3MZeZE&~|RhHbxBvfRdZkhMvUX|4av%}TvW!v}kE^PK0iX8(!s`MNf;H^E%k zn=f@HSJi-;R7ic339a;hMc1Og($nA)_TxY?clg@vx|!AqthC|@{P7vd`d4?*7`=4C z2(y1Md77AfQ!EDXL&WhJ+BCm&9&P(Oj&mYAc)n>ETc4cG?K^viy|$7<&-x3TSihL_ z?k=Smiwj)xKRK3WtcCfjCS%p2sqpKM-N3(klXh2|q4)e8cH-Y%%Rr`&( zEe576E8+}z9k=FEt7Z6k(xLp)x^#ia6;Y*h5gXQG!;-daLhP*PEl;h*INp*q$?LPZ zzb#PSR*KzSV8}Ex>nUo*U(UNln|He3Ah;^t0YxvuxM_1#or|*#Vbb5Ru;LH$$oV`qjQl#{GLI{0yh~8 zoZ8+@ALq6TZtPn^`jgV3XDQydx*9Yo>I46q)F^V^5cpv( z#Ooh!l6zJvdlVvti^Ha%_FH9E`jo-Dw_aS&cU!hD>pkS_l%PaPB3~Y>UUw!WpV~H$ z#iNH&G+U~Rdp+Ext~D^3Kg(6|0gr~EcbgCM%h%!0{bX#SnvO6%L=uWvqp%`-tS~@rh}gimhOO})jAWLKKb0M5(yK8-qw;zxotK0rlm-@Y zbGfIA|DbxpM+z_-%0`5l)7OM)^hWC!6rWMV*3D+j*jS1_u8oFGdY-g2%3E+H=MkJ2 zJfu61%lUNstu!@S%sX0L;FJEIq{x!5;6K?7h4Yp|PNEP7PaV&y4=rWN7k9uP@lL)= zV_}_5!WCx!=ri}&ry47Cb}~PD$@e|d!lR#ma3+)cITIBrYD-Zh$;CGKpGi3vnmKV` z1}KY>htE)oyaXOtFyNQmauJtz%ZT%om%$#@>)fo8^VoI05Q^q4p@yle=;Y}N?p<~# zh1H)&_0cY{sbw@;o;MMwD1fLmVH@}K@h(WLaG=ux3fK_%oZQoQlGorB#)3!T&83PkNfg?vwOj!#~Nz< zmL+=pi4)Jc?qLa>#QXx%OFYP>zCTE(RTq$_XEJElXwV(atB|^JgK)jKx=`+N2P}RW ziE6>;+2f(SbMr1)awzH*6{x1NYa5Gr^+U&KXZag z^+86_zXtS!b)e{`B%At~{;9zSeXb;D~Z((KU{ZK`RL0_VG_?E0m7 z>~f1OYaek5Z>6P)4m|864b>2IYRsba`QHUYl_Z@H)O`?qQa;Z)%=*#!K*hUXh`hzB>(c?DJkS{%C%-w*m6&=Wj`7qDRxwznjF9feqW5G6~>084n zI#fD}otk%-9Wl3~E$cS&XWr5C3+>F;7pPy4<>QV# z;_b@MlXT!XenaIOn9Viv7t?Z~a###T^{jz?wZW{EHxu%gO~sk1-s0_NfZJ`Z$F!{! zaZjKV9&jB;jr+3rvSmE%)wiYLl}Vh`1uaO|(Fa@q>CF4qNMYeUWh|L2K^wmn!Tu`- z*wr+MtuXILed#=MmEDblmUM!4=tK7KP66eoB=TtKqRh9?~JI(N{=n)-X+e{692TAbV7>!8P;=;DwrJTa4o(R% zyuT9Gye@~0Ry8>Jel09d2;jQamT;|(lCbFYL|C-a6Q7nPatEgtQPT59G#q-6;$*(y zp^9N*<60~6H~A$rLNk#P>NQxFrW&i(TY~R1eAw|(<-BWKHzfz9qeh-7*=6})rpF%& zKA19)?`Wg5-{o13wgxRU6;k)#6fC$^h`LU8*gFtSakQ9^8nH=uVdQFz^;`@e^?Xor z{!HQJgk~HaFiiZ^W-IR+ag&A?r4IO&$Ka)&rPN=$jeS?GfqyBNAV6(DNmq*4ee`j> zSLx10-%F*_{dF|s^IB-hwGe3>{l*2PKNI*Svz-O-plpR zZWk1D+E8UYmT%Lkz!!&&#a1DXB(>=r`b226wu3Y1p`sElS|r2v5A;_F8cw)-`cL9d zD04;~{^-+RMLywq>~ZJ{(WFbcOm3_UyAxUnqiVDvao9KD)yL4@`aKx#ZhD^qwfX=R2EIShdGF_Q@#j zt4<4rMTvQB^!!Y>!0+!UV&RmG#vkxME7iD+1#WD?AkRwao>tQI;(tu zJ$SZ`&7G>ttyLOIibJy5+#YkmN{9h{*IP9lmj}POSyDDt;izCPtxsl|L zW?cU9CrOS8HNfcyJ z8A8?VIilTvRJhfBQv|8=-DsNV0F!(S;NdVBap^TG{O=z+;yKT^qF%soD0bUKYMS|c zQuq*Y&?AS-xyKXf+>6bG^$w)*V-0WnM;W5ir_jD46h{WACbXNysFMAHMvuJP!X-Qxyc)sD^SXu z)BN4Y&w>-nP2rJ>Jr|%Ypro2rq_%gPz%(Sn>7C~T(IbQFoQy^;6^_ZKzZZ=`_V_56 zd+8(H(%l5--^;SAAIi|I*#_Qs4`&nV%LigkUBW=u+v1O9^Kj{05$@IQ0`^>$U%t_h z?YY#;HywC@(WA3)eZx%{`{*Im|2hH-=FVUpvk$=Ebs{pm=f{~@Z^X^V+t}Tqcj3j` zAlf`(Ax7L-$a1$4KPABkZ^w?n6Mt2NlS`j+IU$Ygx@Qps#mnGyErx9F8Vh;BEVgv} zAo1c`as$DeLr~G}$#34g6zalr&~c@WIB$79mwrqEW``AXy7odcs%huz%7)Vri_z5d z&X?Ma&okH<1H$egQjV3!FP3L%bnOe;e6=2e^=4s7i3;1lZ4s&K$Z^>z*Ep#)J7{^S zCOnpQ#$l_rK>QndstL;wrR>`Qmfa`8>GKhAx4T0#4|u`nS&gVYIrbihQ)W{=QdI5g&OA5H$p5*9xvYX?Khd93!tfIR`M%? z|G_r48@p0EAYi4vKwr&6r=iHV??Mby@ z`tbp4&8=Zx3tDNt)h+tD@GI%cz!bag-1Eh8!tsxIW>ZA4@_H?!j5)x&O#wZ6+%E!7e#)|m@MmD|OI4r=J*t&B^L6k+iZ9XukP2D^$M3r^_$ zU`Oh2Gxe>5gm2g9u)kGeVc;8_-YZ71%$1 zPk!EWaLhVa^!5p6dDnID$M8`sr>Ba|xc86DWoxQi7cggJMe{`16-3KO2Q@2WBQT1NX&&Knb7Z!0R9U7=~ z{9#t?R>6yQsneC6RS@Oe$bagYNp1D^EGqIj{CiTyUmp@g#W&=!en&kW9_R!9R$F8D zPY&gk=kxPVAK`zzz5-#l-$2Lwo9yr1`vY2Z3rRR!!!W}@d=q`0ebY4%zA9G|=D$45 z?Y*)bex%FuaV1B%?C#-YC#T8eEBC{hq9m?Y&W?SwUdh71h_&?-jlOoBYPvKy!Cq7T z{lDM5o=iL)TRMk6N13n{16ta@zqj%Jj4C`bc{WvK_H)^a5BZpE4eT#>pl4pe6wo?@6=I^$t;HzTUPM()txZ+4F`WF z28$dH9fS$*vY=sAtY796o>D7zg!7~feRVF1q66H7)q}<=9VGerFy@9>flI*K&Gv3?O1adF4 zC@aPuGyZ6@XMX^W2hM^PNp0bG14M%$z}$%W_-AZ|NcFER8jKwUCWdQ8Gq$gR1n)r3 zs7{QLrz~*!>ngY$=Y%2mgc#zuk5XnwvQ<+mxS4nlleHg!`>8-2k*A4{W$Wm{*vItH zX*2tx-bb@8jKkXMI54SAr2wU)T*2oC?sZN3BAtN4+)eFJ* zn*_w0T;?PE`Hy$Y`EQF&EQZx8{gOk+d79Z192xydVLYRxv|)#v!A}Y zZl~e;3+VLw(_}M~!OhB0v}=o`*;Y&F>|0P7Blo`p6j8Y15J5xW@A&Qe{Rgh+?fT(*o^zdZf86o=-ZE4((;9cs-7|(x_uazdP6gnU;hXu0P4V>C0V#Bf zE5_krl3MR~052s@VQG+sR#5N%8Y@D3y>2uqGoJca^NceGVC9f6pseAGU;UvG@(QP?jpB>QTLW z>v+fSiHD$l;wY-LLa8cpYY;zw zw3|B3XYelQ1a0>ACG%?Y$k=@!n2by(ADduF?={YaN!MKhA* z_T{{5k3HYf=RUWWmvB6j#_!&ZC#Hm z*KWmL&n$G1n+OGi#sH660tH{U!PjGHP~aiSJtnnd+kXx)gQxR-K9hih^!!z7Uja~LD@V;)1RMgZ>Dd< z1`AQfefhIUX?preCNaOWkv|Lzp@!@;Bos808>a_Vp{l&owVT@@clRD>>+lBK>R7nc zbd-B{0`HTti3S_j;a%4;RJKTtU%h@8PablE@P7+%zSRsE;Io8Z^;{-Yf(p;ksKW%e z#rSmjG#t9$mTV0<$zs$a(ChI_lzuK#wIjibZu)VQ-K$9#$>nX_c2qV@{XCwI$Xi^s z+S=5q`JNoNlpe}^;&frp@40mDmjayR+Q<$q_o8mCuc&-IrxTPuL!0sktROG3d&dH_ zJFtT$dz*9p$(lTElU~)^#-aS9c>wR2Cr?!s2a(j~t$cs_HQu6q6eq7M`;`lsi zDmAi^+E+z$Eze3eeXtz%pCgZ^=k!H~iX<4Fy^^PH+`)yn(Y(PpiMP-^becmk zM4BtojwM$xQ|=snqFe@11EcBJ&M49vK7;SpcISz=&I_5cU5@>~ZW4wa-a+O|cEUC~ zxAEVyW8ghGAH6Npz~pWgtvyx-**fQG%Z_nX^39iEU(N$^eb`K1db%06-R=^u8oXwS zkMl_M{bSV5E)9LV9?;u6iy*Y&Jb2wc4_5{kQ2V$hn5}Pte~-MS=dQn^Tz#U`N1Zj) z>RKe1-SLIgCDd^}wS8QzZwNm&y&re?P2v^3Dp+X{j9&V!-ri}b zd_M;G@dh|A$l>MOqdZ>yI4(Oj4MMgYK&u6Iye9Q8%vsS=-q+4lRNN))IOoA~cDgW- z>woP-%I$~4Z;G^PuZJ)rWg*wT!?=yyM7sW?1~t3vMh#w%rbd%rVDl(B{=oAJuisD& zcl6@n+S*L8E7}3*r6Ps~h|G9Q5On_twt$8$MktPWFH4-*Ko)1~K6c-d4@^mA2jQNxdCEf>!;i|uoA;}%(9_z&Y zK~20TRGv?qmP!svCvp#ycyw==$-ghI!?PASe4#}Ry&qr=mZ$dP=9gok_>X}42`;!I zGaL$aYr*PFEbm#OhdYm7Wr-i{s7<&nH*=Xy`qxEQ9unt>dP~ijM^zfA*1f^^2L)b} z5e|1J)bkm))VZhHesP_{G;S4j9$I7H2vaU5@a|eyTD;YS->>xM)vfw`wwnTMk~n9a zZ{Nh=@@!n@>W8*ZEUNm<>0-W}{b008CQO{wOcxK?N)_t6Y5Lo<)OOrE7}%r*h64wJ ziQ!J%dSxc{Td0Va%BHdV&hL>*7!%&1!e<>g%meQq#4-tg`**)HUVdtU4^vKyy)qj_ z&9zA|OC}JLZHD3wd1a~-+fngBdJCT{`tY}zN$@9dIsd-jk?$Vk1V&2#@sEPUW8ogk z_j}!-yB~a~avo*;`jr0oGfj^AHG5(o$qvHEqmBC4pXKKBCPRGcT(~=M5;gi7&!+F5 zg)xpxu-x|t3xB#% zQkN`=)2&d!eXnZe&fx04#`kesbd8v7;OeKrl`C3(Nd7?Th>?#oZEbjHM~BtaLqX*6NqQAyl# z;a@P^n1bQE+~8@kiPOEJ4DNsS5GcIf$bVap<(hxmNcO!PTyxfn3poeyNQN()y08;W zT8Hr2y7{z0UA{^KTA}K6GwZf_O=0H?>U+on^d-5^@yq8@>#Z@g`^g*lHY6DPNPF_O z5?OjOus<4PyJ7Cs5V#?=0^+)piPFuXw7Ih%EikXZHD6}Kg2Hg>;<1lc&Nc*3b$iIT z{*So0j)A_vP3h>kx{NU{f zy8YH_zBl44+x}9*9O)<$NSaCPDs~}en4r$jjj*Ce*n`NP0)(kmXwvbe@_f2v>B#(vn$M;ALy_;slOcDV+@le_KIWMnek@h1g* z0<_S3Rx0Y{MNqRX8;Qd>6Y^nbDp?a(BdlMcz|Un`khi{tDF1vJpVy)fcQ^K>lN9Xf zTqS4zX7wMqu62>TH%KLp4nt|+mp)LmQiFF%`rK@(Aym4n+p%}eKu9YKWA@%vtoh1C znrFQm7VjfiSMicMYDi1^xfq!HPZnD&9jXFuS~;z^(qrqsE#Uq;) zyf>{)@R4}i{$E?zAc;CmC3+G)5V=@L+$;zGjMJF5QZWmgl#dbL4hR7W+feC#B(~*G zXY*vrab&v{HZo z$1qTci+Ct1PFIFi*CsKyhcC#}At%VOh@+(Mjxa$>QilgrPG_FNF!FA@3TZBXD~yWt zgsHqkoUZr2GCdK>n%?Ax;I3Y{0b*K=A*>;oN z<1&Q%vca&le-1NAF=I0Yf)=HeMf8urf!<%lQk$pj#rA`u>MA?Z@%`?8fmD7UMpMu+3lM&M-wOKkm7sZ1D7g~6lNqE8XZ0mZ9QNfakwHJJg-eFBvE)Gs z1Q6k*y&i&#-xhv1rn>|^=}CT|5iVQ`A*HgF}e|6MIA zhRd=;v$V;_z3Z{5%LoiF42OpO1Hg1&1W_vJ5+2T(Nm70;#Z*;ySTwOoX!ZX|mYj7F zgSMxWBugq97v=~Z&L?2Qi!hd`-6Yf{z7dythdQ28oX_r`&;%2ezk;v18)?rT4j-ef z;Avb9nHyok3hgS{&T&m*WBFp-T(8e6GqV}Tl51};_o^L-CCq){0VXC0abS5gd2Heb^+JQYKoE+ zr05;(M|_Tj!GuX`QLQ9Sl2K8Gm)pjP6CWf=xZkEq?}h6G&BXqAHt{DbpJ~OCF5Y4h zM;e7%gL?LRpqH>DL=>Labvlk$9th9Ru6Im#$rGIB#gkLk77$q&NG9(LCR+~QV0V_z zWC|}ov7<|uF~^anB<%GYLCPYGdAI6{8dXW+w0m~U_D3w~6|Bft`w!x)FMCik%7mQh z^OfC}pC(R8l7sC(4v<}M-wOeoFM`W$$xfp%U33Y#EpW4ym09Z>n3$c&^jr+tjHqIE zui2W!Ev_L}P7;;b-|ylED`hhEzch!+0q2DFDd%CSyDe^hXe<8vb&Snk+CX}Hip7?V zl6UKEf(*Os6$Xkkh)=K%{41{^y8Vra)R*^U!uETD?bv3rb@5Kpq;1Md)B>=+A{f@& zd?%~ls`8E!Eta6%#lD?!X7?ZWVXu6=NXsN|*3@wc2hR4ZED7==HP*iDr`u#Q>sdCC z{tMZ}BsHPqRF80F*dW$iFULN0J|vsS8zNhu2%}F9z|8W8P^_lJ$tO*aQ*tK;Gix1( ztv(B%H|ntKQS&8|f@`d*>IhkN^`mIrmd{4?oM7*hv*EDYe9`CP1{gLahX|&wLaoyx z+%SJLF>Y0Jd>Flgy*X4Y9=5Q>-Iw1B_VQT}xBn*ATnfd}6Nc`Ts!-?!x*e0g&sGA45LrtwaYiz zw_Zh|@4L~&|7HYUS@uTQJ-ZCwNDU>u4{9aZ^T`-~#}xIJNdB$1J+3TRZ-LJ=K8g0# zU)a6A`nWr!n9b5$#*TNZflc~qk|GyHv~L#^t!Qb|q3%ak8TEm~O%d#Az!v&!?>pQO zTnIDFC&1IGvfTLl1ng2ag~b6n^zFqQrd~USSd9&ZrVoqIB)y7x{R{)kx61r;-+4sy zcqaayqRX=E{IRUP5Iu^0&}^5nq+gkZ!nXvRe=d%+@9o5%(KpInPak8kWwS8Xzn(m| zZz76P31HzE4h=zy^xwx~Vm`i+-MVFo0bR=QC{N=1J5b2R*A~E$<5NlT11T*1BjU+x zF?g}Pio7`L1EchR!jrH+Fy)mF7Hs#y$V_w07&!cG5~AEmO2VsPeza*Jr`l^wg=?b=2I|sVkJ0*Z^mJcuNnJi zNFMl}AfJ7zVA-R3ap018;&+Ra=gkLjhW$?15$Xwxx?U3f<;w6qOO;tP`$01)CN9#? zpxNAtd|$9fJkU1|rY6YZ$g3xCYP26F%9(QWGF|q%^oH=m>K0Ut^u|t}jH{F6!FQ=T zbsi%x{P!&pA5D{it%pPLoXuWz@Kk_b2bYm$Pb8kF!TKc4@C1q7a|P1%hmbudClH4f zq0F%P8BtBsfOpICnCvoVj9tGOzIUmTpoB!!{g#coM2uxvLs8KH(r5MEilG8dY{NMr!sN)`8Z-8 z?E`+Q*F#EmYme6GvN@jm?W~oxUV7SLurZY1frc9qpdT-ei<;E!Z z^ebABelEwSSQt?^77GV$=F#Fbii2A<_?sU^l zpYE*|oE;yaSyLR_Un7eXY+o~*iyG+Wm<&1D5oG1&NGug|aMSxGEX+X-XUh0P@RuTH zJ+Fc}-Hj*jw#DJeQL9M$i4e#;-X_MRj{uJVfP|_MI4pk?3;i(xqOyH)xQhkWRPBdr zzAuQ%ISI@8?*S`ZzZ;E)WDxVINomFdk-Iqx-!6YB!enD;Y??~`yo(Vl zV#1)j!~_$pI>1h&&e5ZBIFA2Qi{7=<;J+L#VtOTw3?EsBn>8_i8*GOw zP8YGEr?n~vj)}n4KXPE&r3&)rHercH_3VCSDIN&0#2wKYq%PMAR=#n<+ff&B?vnu| z&^ipV$4Svf4-3rOEWk@iF5^dbJIvW^1K-wXqxpFwKBVgw7S9_E6?q}pe@i*u`m_?# z4*o`e>v^Q_%-!tcv?pSg%mnCjS5v|f%zzd(Blz(7wRlf%CzOu~hpa?vob;nV+PSn5 z`eQ6_U1`SJ_nTl;^HEzdmf(%6d&s1QRL8)CSYg(sQyU3! zgJer}=#9?C_FgX_YmUOWhTmvfw4MFl=Y%2sO!499bfR-41ZQbYfcV#zbj^$`ydr&r z&5)_aZ{f=9T|fg1`Z57?2TsHw?_X?3)kkr*y(%$LQp1Lb638kI#@;73%xLH!p(6Yu z8FX_yv-MI2`~AUSlK%;}s?OxG<23MB@eKU6v_HKvXCbvMxsF?Om7wQVHO`QD9vZJU zki8Oy;@q=%XkQnD9y->n*fEt9&*>w{uSV0W(oM{@WIA`-rBpSpsMk^E?f}d+RHa=5 z&Xa>Hi*T7rtnhnV5=yHK;|KLu;!O`-I_=Q~R(xv=(lR%6>emPn_igBJ?Zd3C>@GNM zO(V7Y+8|+V4A#}il5IH`gdXXeXe}=*j%=@nhYR)S8RJagCw%cgGh1$ZXrAK;m&JHF zzYE{>7P8lW*HQKJ^5jpPIcR%Q{$`~NpYyy4hfVJxmS5sgVxSST~n|f{02P2pJRR7ATAqQ z0TF(Ec&5Z3)UJ0LRfmtJQg)pX-rPf44RoQR&KeeWXF<)iaNcxRfg39sN#=f!QTpa@ zco-#d1`W;S*6pFVsW>0{E$@QRoqF)4S`}Q*gn{$zt5=ONT7ehhq=-{7D-_N93Gni@Rn$b(xU zC0O91Ov5GLPQXN2uK7HY_d9!$+YC|RRbBV6MI{Ueq+W&4p;heV+ausPy_@Vi8U}Oj zAAx~}X0&^fJ$E^?3tdegz_6d2A@iUCR{U$imPTtD<+lqg_eNr}bRg}SERV*swOJV+ zL7&FO{E%Z2xfuBjPUPG|bAz+szb+lScJ)Be_StlD_hLxhgW~a=7Er5~;o9z+{M-Gv-si))*4cairfaWE@|>j-|9dS-8aHsqYHbYJuxZDH zm4QKi6E=jb-?DZ^P|%7UNwG@Hg8WtnZVcWM6tZ$l($S7t~q(hR6i_6@91K0mP z=1FWx@{M+skd%=4+H#XU{@N)@YI0@GU){;+q#SZxv6Tq|3G2;w!V|}p*!i{}EGT_F znPTXL%?jIL`DO>QanS^Pekz{j+_NNOR)&!$rm>>4KeQq5{xLz|{C1X9JQ|%z@q5)O5J~ zrjUhADu?U$tkL?FC4RN}%&G#tSdEhj#=p6YGIwPKE)vqb;`DL4-6@}ac!0Eq z=e@A(j4s|@q{4p+Y%%Tnd0Of0O10KmaCt)wBFd`~Ege0c{hqv^J^eXW^frHx@NfwT zbi9pl*VW6o-71>vENtPU0?*RocQ;|lm?(ZKBM~|#Jz=uuzA)HJg&1sIjwW7r1s^hR zV5sLS%sFwKhpf<{*H0<%(x8#F?etb|U^;||jAiKJO<@qJsz-5`Jw; zGC$s~3(sb(qt(inS&Zm7T%Wq0{+QW}MBle|p1u)eD_nuOtG3~PH}CTkKlI_(Cl0r^ z^^^A_Zn4}aYek`gI3{?fO>O08 zPuu*^%hZ%EtpVY}5$ovO{2zjaQTti%{B~;g>OFJ)K34p*BLyVP`_OuXG+ghR$}dh- zw-EXS=+aWDKK$1*Fs^c%_Io@A z{jFgFuWdi!jL@48D$=JfzMrEXlMg}Q)iAtaV8Gu@EO2<=_zYwgcF<2J-Ox5#UeMR+ zO!pHTKEv9Vl^NFyYzH4@yZ_B+-~YzaPuijM^vMhe$Gx~zr5G*-UZCE8C%~sa2hnZh zag?{w=Iw6j=;|_>1|Cl0yZYTPx#b+COVY+rUyT^TWF1M}rx>>Pj}d!xW-=R{D2E;H zYOwCY9r9T>lIE?wh4T8X-0)R0P1pUx&)zb_z!U9|9BILo=lJj!Jd5-Pr;y5PyJ*|9 zv9z+uhW_Zupr<5PLF$z=&>YnYKEn=?f@n3AJ60|FwO_<9{i?#u5jx`0arfxdv~|3) zstgi;S@U%dli|lOP5Me&kv}}2%~wy_0G?A6>5v~+`O2(OeDRdQoW4-xQT35feQXii zpwUlcA*b|P0+3MA3rpxnz#m4vmXvFT<^?9c)M;qi>x)Kxsq1g$o3eb z!);>drOtL9?WhfvJwQaME>v&g0iJ&=f~H8l$I%zO=o{rI2)_53l~j1sA^WC@9VIuw z?^7mV{IZN2#v`n0^u{y|W475M9b`!g@3=IPC;l?wiV_d8#FwGnoN&y-I|MZ^iMq8< zpnHEQ?OIq!uT0S3Z4-_8+M`F%xTp=6-h7W;>qEJ7(pp|wd4Ni(a2`~XK(8K- z;qEjCz~ot@FulpRVTW50}unuJJ5c z(q3Q}vKlQq_V7D-;~Yv)Y=kAci@AkICU{llVNc_IbnH^&pS9=Fi#H~t@X>ba?)_PG zV)S47#juRaYzi=UVof6PV z-)@S|SKF|%Y!lS{RLytIe?-60kF>)o4m4tw>Dc{KNxGIPCg;B=m(MUkL)T?2yOe~* zLxp%Vq!4}nWnk~epOE!nFFm*;pX+~(<8S4A;OO;qYWDdO-Aax)oZHsIgB>OapPEO| zAcYy+I$vyNqkcFuH`RQ;L%kOS*U!?(ZY{odNG0BF^Pw3wKR{swXX1V1c#G*~?Elx# z&Uap;<^5CnThTs#RWgqL`y&>;EmdG|?(Ai=vJ08Sm_eZX;TT@~91A&Fa{O_@8-CNO z7#D;(^UzWS@g0RLyhkRr_VeKwR1!yG-tZ<&&^U@apAW@SZGEA{3M;B%Qbltf4HqU( z_QGkM1<)}img_&dj}@^xczuo}{gTr}(={)WX$xavW$X~3Xzfh{Tj$kWr$qP;FuBj}0IrTO> zZ}@~c&kLdJ)AL1Z1%`O_?rD11c{^=bp~3B6Nr?r?9aLC5TI{YX!=6>t!Gyu{_|x2I zY+4~nJw7$nRwzy7tqZbHtP@53PB=n-r8NKSKO2tBkKvm_K9?9zFb?8`f;1X3|lxdAk-=S$p!v#to1cIDx%*>%!c$ zdIhSo|A``Z9i($q`-tXJN$fv!oqvuE!$(%C^iV`QUURpnPu_oqYXM{7nsF78k~E{G zCv zTaMvrh2A`%)m%&`|6wh9NwpK@jQR3hAKrNI5q{1-!DJ#5K(<7KPrrT$X1n}kr-wAd z=vF!A+L6l~0;xcHTo`#iZI58a-Un{58#P~h-beKajmRN+^v zTG`3bZ*lDeDgNc4BjkMgggZXGA^FwLu;IZF7Nb20+{Wb*kH82LnqJNxYpoD`nZ5^S zyotmGrN!{st_XE~eLyQ|DqlBrIle#KDYD;^0>bu@!uG&t;yJM^h-jx0T|R#nJDZ`1 zVP107^5$CjD!`KR~p+s727< z^^aUFlNabr>443i;VdC{44eDspy1q>IF?rP9q#L2Cbn~Aux$NgK|xtE7Untf>LL|* zbguxmE)Jmvu2*n^!dbqeEuFN_E)$riB$K^&{YlN5-*`R2lZ!s>q#Mb4%xROQF(BaM z`%T5$bJlYCK|lFRzb0I4T1N_2KjwRmRdV~w9q>8jI98O5;p5KEf~n&Mq2}~#tVmt} zQ^w53*+!RZXITcr8jVOMT^R<=tQ^0sTu8O9cY=i3dKU9z6QeE{@uJO04E-~SmhCm; zQU$g6s$(l(mRtc>-V3Q&$r*BcnF1~NY%aKO6UZe8?T5t$h<4c@XN-H zPq|nF1`&_IWWEC4J7C29z8^r{+C%vEObCv;y$Qp0_ClXiEZ*NanZ9difYGPNaK~Zu zVcNQSoHsXl)YItS5WzR9GUN) zPQDv0B#Am3x#OS!XtB1V?hEDUoyQZQ+-xG%!yCd5=^?`I;J2W^`7-UemCSG0$>9c_ z7W6L+hvxIcd6Bs-yZmn^Kk!fic~>@{pxnTc^WE9YJ_FLI{D|fCTHw;QLU8|+!z~YK zW7wzRxbMW^fz=tuyyhmcJ0&(SvTYOuv&Cp`|C^Omsqqb;)ahS~vpfow`N1R`nh`vP z*8JQDhWkx$^^#G%DmxsQ)K;qEG!M^t*W#4tfY?2NX*P}EGuMZL+MOu)X_NsWbs2DL z?UgNE4y1R^=Pq}9*@-Wa zWJ{}tD84Cxy_*=$cid{^-Tz(0_4C&7P@C^`wB#ooYA$N-fg647u+OGbX$=4mkpjBh}t*kpZ=71at`MZPm zJG#J@iD_(=a}+*aR7_LsMsxi+>abtS1L&1%w))y&)M>R7>=OSHJTwg;HTmP%Y=^!4 z`YJ_?IXMD;y*K9$9zl5U%T_)i$c}p_JQ0nGyib#3-LXFJ1Ce>H#sh3RVO+{g!Sx#% z?1$lR(k2(pHb(rQGn1b4%`axsFk>PA?Iyyw5-|+l90JZ{0~BYy!(;90T-sm-Unw<} zc6ryJ(rq_1Jhq1>G;X4!-_M~9D`%jBJcSREGib6^COspO$D0UrO3t22(|PE zivsMn@zTfb%;V8S%>HG|_O5VcvmDRSJG}yG^W2Gl(4Pk({Tt|M0~KMd;|T85s!iWL zsuu~fM!<-;I>A5T5^f(H$J8?i46^?tXu7tW`>wbQM@`3K zsYyE~2Q6XjMJK?B%Q!4x2>m*)g?G-7!yv5&5_v8Gy>={z!K&&Mjt`|q2`l02Xo@!0-5-;wa+@;rRVaaLj43ftcqQ1V{~H$1i(-Ln zBBrQLA+Fl@FzxLkSd@Goi}_nYub-M&gB!u@;8@%#pUNxd=JE9j{&2VRH_`5RPhabg zK&^6bs$TyDMtdmHWigN8gT)fa%q-z&{bHa(HICD`X1v&SluNoj1J^^xXo9c^E}fds zl`PH^qe&n5$iPf|vGWQY)j1uegvx_haSxs2c88DNw}`{er@WM=bZkd}TI zcn)BfBNw()Gv_mO;>>km@pG1F@}f&TbhQXSSY#6Crdo&UlY!(@NIQA_RuvcIhtiTE zYcaL5l=qG3!SJohyrDxyXuQD=?)k@Y3!QM9clj?ENt@85HQS&%G!c3GGYFcPiy?~~ zS?7anR%o~y3s&ty=jLP7rtrn3%JV14fm5xh*Ce9jOODdaM`w8AfqNiln#J2|SBie+ z2x}M2b%eR0NOK&bp)g8URCGB4b9x`~mdy=RY3(0=Z&YdRGM!n_Wbg%*R5jTkF9wL+L{vl@SHB%y03`J4;x7>hMS3!p020Dq9VTKbQUi? zKb0@soye%G2t!X_gEQij5F@Vz4!i%buM1w&ierWN`0;Jgju(k+lX4AIEeRH_t2<0X z=YE6rhAQ+x>rgl?F_ie!979W^W_+;n44jvI0XNrG@Xl|JaC@{0osoQ>4NiT7&qu0) z!MqZTTI+eqlC|Hx7DQfg?g}=d_g09Rg=w@TV z;#d)Wkj!R|ml~OVhcw8U8_|#ny7cyvN&Kv>6s`0yq^)i1Xl3shnm9{`hkr;Cyv$xe z#;ea2&{Qu$^ulrCO|m1YkLwstyyIx5@>b?jn8x$24r6}DJ$bK%wYcj;ERK*9-GvS_Jir39Bk0u|2XOtlIO5#v zO6%mZdCD6jy6Mm1fwTV=_SO~DM!VY4&vU0>#epuacSx7MIBP9%sXxl6KG=qn&fX@! zziNWMb`zQU!W>K@^!R}zI@~uvk`B6dg8o{#o6Ekor;js-(L$+IzEPtKie5U<8-tvn z==6kvbG(E*Pw#aYu9i=4Cmg&~-XKd9P4|&q)0~On==CIu$nig+@1gcS$FDl)9O9pa zQG4q^8Y(l3Cq0p(^^?-*-2f?+GB^r5Ob^034O7Z&r@)gYEl6Bf2O)<}(4uB_-j;re z=55(W)tij@Y->sJ#u;~T(Sr&i^yx#Vk$S=u+{AB?D!QvgPptP!2_C;yV17^XsaXCi zvHCL&6xxsJkoQa->OP3b^174( zeicf0^{B(=na-@DT+APpmDA55x`NR83an0jtYELMwP;;_9mr}9=rbLo2lNORzW(oc zx^+nm94))W!iw);@SS-2WQ-KO!S}tgSs)x@>QokP4-BpT{r$y64``U@dH$`}I)k8eYC4lKG=A+4jMx0sSff?1F z^za)~I>~0XV3&<$9!!nZT=Q^hPgufv@XaQl8N>X5m;Yl1+!T``)*f`$7M^&<=S2l-ZqNs{V;JHB8YCIf!aFDQnjnMmiz92+K6HQYO;M;|l0q^}rN|t#ESSSYZ(`7KqYP$_x2AOiYtr=t5x04qLcf%Lbb2t9 zFJ2kVY77!VS+N_=A>pI859jeQ7wPd)ujysyn^4g@n@(Q03ZE_6jXj&r(0^k`@tl93 zX-exa&?*0lGfqvQ+2Su;|F$umcqfEbodez~HDGVez5pwNm$57N;?Vi@AmJP7O#Xd$ zJ$<$i3gJY=#6bO0mQL>L1~T3K80F4##FgC%R-qGmO)z#OU8v zw07nZ(e{p9=xkD>Vde1d99@|I-zY9Cv4fu7{elY5c+s*= z64c0TC~db}z&+k4;IWTBl-I~nYrSQBTeApGghaDb3FUmH;Y(^mO`+&<1z%xwgbsUL z2S*QH#9`*m*!|o97GI0zH;g-ptkHi+`}X7Bkaxh39KdX^%1i5q=(1(iCj|%I?h)Ak{XBc*U}L=($VJjPVVm!k?lis$D>bJHLxx>52sF4-$BjA$z$-~T-&fwB>FgrXt$ z@m?gB-}Z|vtX1LqkGIi5|GmeQa9#M8UM-q`b__SWna(n-g|s|MpARV}qYE#)(xla! z`QSv3XJ`9x+aP7`)6)dS;jg&&+Eoy{pHdG~ZQk?Z0{$DLJm8OZrF zhQHp|!EmcIYI|loANj+O?>jY*#GC;B$#4su*K(R2*}sN8mO8Zc;Lm8c?a;~(fszvZX6Q%g-z^s638cb zz;fF)Ftnlyg^|(xbLAxJWW5}tuHR;EhKs1Nr9QvjS;it2HnR2i^4Y9g13a(KpEO!0 zqj7NqwaF@mHv2BJ+un!YZkoh@zig;IILj1-kyD}BXEY!DQku$;SVm7B$1=T(xJjp; z^`4hyHA+hO;O2l{tEzx2Qr^*p*=6+Unv=YK`c`_YCXSPZQhe-j6;b{2;rvngKc103 z34-sR!Gv?MsJrb8hNwECzRx-K`Q1;bR?0^y`zy5bD4j8te>S)UI`wtr^wMo~ zm)xazvK_GS7w z)@MEswT>&(iuyW1_IH(9`^g6h#_R$)<#cj6ZGuRAwve2tP-FJ@ltd=i?C6lAYVR*Ih0KKnG49n$`Y-j6@A3lcUWNds%X zboU#&{bx1tPWwlTzRU9A3c;-Jhy#^T5P;obWxAlz3xZ3&!f3an@J(A99?l=mYhtV+ z=GYD{ZD)WFiUx}p-kT+~nKMMVSD~IswvXp^WwBs+Z56jwKfo8=94_p+tI7W?|Bj~f z)yV3A>3Gg$HjPy;hLF}C*#7OaXqZNWDDXDNQrY3KGA&bdCdz`0?3Wj~dw7%ap?2JO zaV}rEjbYj7seI`XPZG1+1IWEkaJ?s;8PPfXkbR=yQ9(I9ubRb1gBlH&)8`e}20>N* zUUqWNE0NYcN0yiVP>?z3zR2UmX~BNKRiZ!Q3q*aH33p$=8A>+X0{Z0|%`P(+rnQdd zHLo{=iTVMUATK6^wX0CCN1eW_GDfeRhsm?d$#h{;7j1Afq_18X4R8-<;v1O*mZ`&d z${H6Ur&@@n4T?PM$2x9({wHkx1H57BWZHKw0s$ zD+a~Un&eMnA$@8#13Xd(y!^!irqpnl)Xtg4WM4gH|3)hErF}d2xh8)Sf8pe%V5M|W zo^lJ-EO#-(4vJ1Y?z2w$yR7^1Vq6lY4r&v$`5L)Va48}VzWPSsusgL_S#e7=RAvh? zjT{NnI%cx^J$E2Yt`p`IRY1gQPqs?Vh)*8I*}G{`4l!$TagbCi9yocP7uS#G{*8$+ zD)}pOQj_P;-<8o&Q4BFI*W*LuQib^i2BgOBAevPu3QvDm6w34&b0I4e&30)8xiPUc zOzH@CD!2-guPS)5N-8>SlNV3d@#ANGEbx_LBsI@_$+wgsq^&Kd8w+osWb0*|=&!Yt8dd5Ta{mh( zySxSi$5%nR$tR>Y(m4C2!vEBqNBsq>h;(oomkq7tb~!`o<5)RttxudPF;@XrTS*Bw z$+S>y?+JWt_aq*btBvLp+TmWfFDzfZ28;Z9`Ro8S{?vOvF(_yzX*bW2c8_12>>BG8YU+M&b4sjSlWnzlmJk zZ}MSi4*4>UGL1n}?BS>gwncLXxI}c6&e0n$=35svE$K_9^1`XQHWVoeRq| zJju#*WT751iEv+S@``&<6MUU9TZ@aO$aUR{h&jL2| zw}8$e`&z$!@nCB!WKN(hN?!ehg%qTdrF#L!6=s7<>vM#FPlDJ}gM=SfnAB}wqz{%(S}v3@!3H2TI? zcY71EVjc^_;}CI>qq%}2c*hUq%(|a|lA;r=X~~2o)8CQBCgZ7KOFXR0%7XNI7tx8Y zrKH6(kqvd10cCLz8Qd7keqOl6ygJA7t~t>}Id~Ud$x4FVvEh(0d@3L0>rFS#IYp;= z{U`1@8xDUBe+!1*pC(S2IZ7<*?*_96i$tTwl%rGYCaz*`MpaGnP;<5^Gm^EFMD`J%}Ycn-Qj{$L>ZsBL1~M^Mbx=%Qt(f>5?M zK$I8$54#VEpy1$4qO^KQ?R@$8+AW&>f&%H6u)yXH?DiYP4X#~>gyaSEht&$cX7~e~ zZ0QNDX>wF=oC$0APaA~}%2YJC39dY0pt@3*=D8K&JH;ykKYuGxYjhqNr{7IR+?GX+ zPcz}`NikbLcMrAlG@-kbqUnN_e?{ZXjm55hMSR5OCPDBJOFl965pPKPz-59nX@pm^ zNNtF@=%wmxRyUW!>UUjeJIa6zN#8D7J*JMx-#dh(Q#dimM~EN44_p_zbIZ;Dc-4_y zxOw#poI6^IcjD9WiqBG_6tW1XZCgd>ThHaQFQ*FzvQc2&Qpy9muhU6!hD~x>U6f`ZkthF&$d7wl_1&tyy9@7M;Esl!L_bun!9ki%!^*S!sqeg78SX0wT_2iei3q7U%1CKPwNZ-m!!S=>i06Aov6gRyJp za%-DH`cYDs=d`EN-r@Z;Ro0*Hw{6G8CUH=}i*U%<6i^ILrHj5D=AYlcg{{3G+48ly z_`S^>Dw#u6pDEVlaNu0lkdXxAx2I6|A1AoSw=$Ykl@B)3sYEz{d1p*N0^fGe zLjO0N5ZI+4Oba#VQ?nCsNTn(S%Pbbdm?nHv`$D2DroiJ) z9cnE*h&oSRL6x2hFrg(~P@2D&M}@R-wke)RXI*EHpDXZcM;*R1{}Sw+tOrm2$deII zOMuVQDc{fto0XTms<1QUuE=9&`54tGLUhsTuP*T zZsK}{A0j0KOK6s^6m*DfQGJ3hsQh%}3|V z{$_UbP5HYI>fF0gp39O!v<9|M-RYkNA3Ec~{_h{b#-D@P{Mm_o{H5<8&amX?a#CnR zYc@<2yx<07pIUTHcpYOk4xt0{-Pa z20dEFpmQIMKOsk_=e|UvSq5x;&jIq~Z5DjmRppPn+;l(@wy)?swJYp)i-<9*LZB%$*sT#hkKg7rM1hYh;}?WTL61SW00E^@!Mnl3nY zk+#a8qD4OUxmW!yE*(FX*F8Kah#VJAC%ZKBy(K@ng7anW@qEsJm)MEr8wT-)2^ykN zxjW#gMHzV71>x7!!DMxG4X7@Yfh{&~@IsIWwfS@iUgoM{e|8z)ZTt{|^wWVz zY=_fM>Y&#(1zZd3$V|mliW5Dl=U6vTeVa<%j=YE0zYoJNPfPk<-;&R2AIYyyD&oI) zkAQ#(HLmVv&z1ChI6pFlIXxRCsP;WAQki?6^@^wCct1XHp`aat1|SUaDqu(dF+ z$AG)uP$1pE9x>G=)gY(15L}vT$o2do?AH%XxZ->mzL)H0#RdzY@!x737NW@emsOCt zBB$D)lPA*eK})Hz+Y4697juuDLG;_QXuinbh6crp*oB#re3Vd%-m{dYb43a0GIJ!? zbWNn8w#}fhNfNU*4ru&PQX6D@f`GIGIeBIT8(n&d_<2TfuamV%sZrbZ|ZC4i&w>$$x1(A@HmmNNNovM^>9)u{0suE-YaeHs!INu#2Fh zlL5n@dcv|kJ?e9`9=`=u(b(z)uI^opnhP|zpO-*f^ldAjVtxt7$+ly-EaCUswqS3G zGHu$FPBenI&wjC=`pHj$4XGBLcwq*>UFlrUuF%zT!{% zuF)T|N2z(bJ5P8}3{S4efSqb0&lv9yAJ*-_^M*&^S=wK0zh8`f#cT0zwiTtCMf_e% z2EVDlndvMXO8>SQ(8Kd0uD!FfQFhDWP-Z3Zv>ol5vyh(oF&X7zjHzgyCQm%JhF%Z(&NLlmFeLl9Xl>#lqM`mA z*Iq-iBtrq^x_s$Bx{gK-Itf#yw8aiB#`NjBb@YY(G)xz!gCJ7CMO7l|YZ(pqtx{_F zxn%sl^$FKjQ|Ec5QGC(1RJuJp3Y1Ha(y?VGymWFi=HM;ZSFZzaB8TvbI3ZuH6-oow zgz<$#l3;)CFYx-2zze?T^Q-gHNZ0rvb}1Zurae*WbZWx$@8sP-oH_#=zo$mbPi$P3`NYC9>sfO zm4Ku?9^f@8)GM-2< z$#`JAw!lxdwRVzwvgpY`J5i1OFCMtXipuvd<*j?%=&_w&s6c)O7=QPGRTF%udBI}p zb=QYEq`AS)b-^OCZZ#+ejOJ~J53&cxb9nMLD;yf(0_CM;Sbb#{y*}zO#@l6(N6m&L z-+-d5<}ND#R)J33>_Pk7V)*UtZ8$xuz(KNr(kkD#m=*UBA_l8d!7^2zw=I?r-}C}r ze{SO2x80#ns}DoC_j&x`VFcm9XUU9@nyjn!IV+PaV`<0k@#IiFc0_#$Y#22HO3Fs@ z1D*@e+2yyWY{&vOz9Nd5f5~C#!~X&V1 zFfKg>ACK?mev7B`?8Oyu_|szQUG;*`8`3P=`YxTkv-c+}?`WWri79&81n^zQLg9XK z2igp}11m52vpsTM_*h#-80R%pXd$m9o}Tg?BxFuPN!SDas~oxX*>xC^=|)@v3~1w> z9sF`%48Qp(hP$Raapj3-n0l-pCh9z4J72m$?l}oAs9r$F6jx%s10x%rEaWqs3g`~& zlQ_O$Ki$0eG*<531rv4OLK&ILuO82$@r$~^D;arMkc{}+fwQ9IIlt-VLniF*+XPe| z@CX)d>R~g&1yn)x6U~}Aoi)66LGZ5Qi#)%>>9!}Nt+kp|sU9Uc1HSI=6IZa?SP}kq z%p$eUHt6rTils{#(fFrsc-J+Sl8jXH=640ij%#EtQBR=9=r|T^+Y6#~U07D*0{@n0 zqOr+-+&|cYxcc8BH~%AaZFUEY-?0t@ZUOyB<{ z^Ca<1akx9L9BeB7lQ0xE%Jgxx^ukLk^97oNBkqs;^*zQ z&^N7`TzE^FwuLzI%fmy_&>{*xPs?J_*0<2};6UcbO-L=vPJ>m=Xb3gQr@#J1LFF+q zySQf@&hom+6ubAcuLn};wUws_c!L}L+&ofHVEGO$zNumRsiEA)ZW!%7<-nf49>CyE zhj~Qd100g}fcY+%42v%uU@3J29>LDJtjNp|FRy(h`0vwxlJGl~>s`!-5D7)TY57fP zJ$HyMvN*=ur*7qM6+Y1k(&NRqGBb(lgB6gr`3|`qJs!S$m4J${Mr6>@fjinR^EdZS z@D(cq`1dV-yl2!eT-Ugax^zyaQd3Vdy%l0Sp6|z}m9OLeGf&XON5kNrdm6oNtV0Yw zE~c0BJ*aJN8E;BoOkYq9e%#@Q;Kxff8fzTM4f@Y;S8+FOIk*uA+VAjOVx>d>v|yH5 zWkTF5v+23GFjC?(fd`yuMy~>G)>D*21jY4&LDDTGY#S%zbD9N3i(Z4qSi;YF#^ADy zPW*OjGn{Zeg-iRYd9I+;;p~GY^u?;(JmFsN)HM-H=nio#2|YQW_xOe3@7xw_{v(Y- zX?eW3?l7L-W{ojzdbIZTb+X&%I0>!r;Dx)jgqOB1r@0E+RMsSiZknn+g)htpa&6|&V~9Y1-f6z7CZB`wFaxNm|InZ5EPKm7I(4B6#Fs|tRjjAJDX4*NmB z%o)m;8$W^V^R&tDoBwcIY=vmYuu!)6NhMeWALe_<91)E@V1$2%4(8#5^HAGY28Ba2 z;qSSHaCdSjbY07UyS>iP(3S(9iAk_E)0TS(_tPH<9`st`5`KEF1fQ^JA$}RJ0?M_@ zqCGon@mAIv@*x0dW>x~cy1f~m+gxF7aRb?XcX`lAk%m*{lX%}*6QT4&hTp~)@f5W* zJo9ra$Kh2psXCE6>^Lr#l@+nOgSD~JayR!lz6ZY>e-a(JR3NJOah}R-wBQcmx*Qjt zAJBqV)6h0Ga#D5=UB22tuyEUZLEYGRdUfL}8YBLX-nw*+URY4XgPTM6tKs1+Y^M(& z`u8X+;SK1y{Rp2q=Qeakxexqh$z=b$PVqOXE(fa>7(iS`0=M1Ta&VY6g_`?Y5AR4VxhQ=wQ6VbNo&4>-7F1w z_w_WaJ9ZOim#Way2}-bErUGX)?#BmqM|oqyUOwU9FWz+LC%lfRq=(N~3;iL7Jysiu zwKF(b)b{I>poFzviIu6a?;>U3=| zc_4Q<^MyLPhKtzxW%}ZZFJI`|&C|Jl;t{(1iX6Q$CkxGOwa6l+3|#hk3`|^Fjk|Kh z*f9J(_%%fG&A52L7ab(rs#Zt$nEQx4mf)!=&R1^775868IK#H)uqakp9x zTohPhQ|SoWIVSegl z!SU>W_(tA`(hw!CBbtaS?;eIP0S{sGhXGHowUqx^IGY|f4C2q-_duWEA=8s^N5>r%CYeHDZ97(} zRMEf<>qWDlO=mu>7pZ6<;jq=2jDN-G#ja`;1fA5}Hx&!2^g7SupvcC@tLw zUsS9`-`BYU$1hj$aMC+aUS!V=hv61mUFUg(hP0Ow5hC>t4MGwUBD*B3kR+?2gzKDxA|;B_RLW>5 z($xOEKDXQFx9|5qxPG{9=RD8-`FPx4PJ&&bX;|rsJjlFW$xd6hv5alAVMtRH#X2im zi}_IeP)y9aykUM_m5MS=tr?=lhPrIi^JL_?CQ39lISaZBKxwk#bO-WQ4y4O=J$9y6|I^7h7N@ zj-7w)#MiF-u`Z`s)>=Q~X~EoZIwAizRiXtT`{4wgtC270a2cYb#Rs7Oxg^VWm17aZ z_tAerhiUM#Oe{e0bl;VDx>Q{iTjWULD$fe+9~;4r$7s;!^LyB&Gh!%9_bLl3GqE?qk? zAJ6T%!HUp-OuDaugwE{aEJu~o=_f5%=d4_~Z1a%hywxMx>ap}aSIlpczQE1>QqNW8 zOeH;#h_^mbVRq);__|yywwpDNof*@GwYIkTz6HX11V?8LHPz$xNK@&dCv~K;%B_DAAKCPEhlWFaDQ6QUh@;9|8TYn{v_B_ zjik*sh0B$@QONbbba7-4sMvY3#<(}s=k{i7kXHtK{sP@UH3qqqM6&I-+}OXC2<)R; zgQb4d<8sXl)T!w*G5MFlU;Q{A?6&cM)}5x|1{rik#6d835oP0VdeM324Y*T0hxVr> zvV}UWY^$~z;jd+rvxCX(&3ifP#yQ!xN?)Aj8T!(r*Pn@THQwr2CR`bto zf=J?{7C!f!G`Bzd2XYU2$4~KcCMNN2utg>l_R&(FpL!F0Rh7a^Qi5sQ_YD4Fy8>EJ zc$tXn<^bzAz!!hKskN}(!F~@Vu+^iKSY(v2!-7+Q58Md9_2)t5ksfNVUkYp013)W! zJ9LC*lc1yH>7NhJNZ|vXh-p0}=iI*$zejJ$q4W{{wO9~UoG@9~CHEjsi4HW|GL4St zWYyj(Fn|i-dvngV93-6;%qwL*s{L#EftQ;XUTynqcP;aI!UaF9B`Z5!sFS7>?SG|# zi%VkR%7kv(@AVXZ)WnhhRP@-wsY~d#(EHY0Q)Qj*A4zOu)`LUhW1(eBAxWP-0nfi* zhHl@;!Yu41el$@7uka4X2Sd{Z%gT4cg=u$T)#=&zQ^0*tZdHNJ(N}5InN&PuQv`NM z+K4M(IkHbTr0~+P*&r%6mNn*$vF?`^vgBJJTZ@$B?VhV+lXC2i=tvdPF{%aaRLBag&#QLMcmo z(b)3~1!?XUbU}?Tv|W&6%4-*6x0qf!`@;~rChVAYlwE_B`{LPUvr?4Y_YdaRSl|zn zw-61_b6Ee?CB8&&65Q_@2On~x;S;$a+$Y()@XJZoo#n$-5Qx5H~P|Ha`r;GLz@BB7<>EsjQvmnycW=yUnn*(q`7<5=#&I`E%*Z zF;l*wgd_Vr*!mOWXouTQ7TIsj{1e{7ej`ghU$>ZO58A+E{~l^y;EJbTABLgTa>&ff zj^cqD!+|g~pYyH8cW>TF-OYKTlI>m2=>*wF-7gNm8@4nG(5Us1N~gx z$zCXCVAG=)AiC)y++P!gVg5N3chiQzF~EW$VE052ZduvLER6K=!zq?fbK@xcnS6*i z$j`#EMKkMis?6y9jd|?vDksG83J+1!jJgW)~mTq2F6a zxWX-lD97R;HQOUX=d43`=7FQYkDP+_TZ&k2a1u!EaG~`w-$+5?ENVTymLvqk(dcGz zJm4sYM>Y6yYoF~AGV%=&fFp_Q?CVtf+87ouRRgItpk_>X=} zF90Gj3A)r8(V^TnG|esssV~Q%;(H2bOP_?nGsj7p(rdChc0H@{v0!0~blJjXZ;4-? zIi{8NU}{r`9E0>=S!xz6)aj=;2FnHCT&5D=chgC(?tO@K`~ahhsu>aJkoYU!U@2im z$InDKB2SHZ+@Fn|ilv1;ibQ>W7*FQ7-wfD;U&Ra*aMq%7)L~yl=2l8 z*!-QknC$|o_-r`jb_r)fA*#`jWQjJ?O#f63b?>dCS05+i<|t2Uy{MNO23g|er$Q*R z-Gi5%dI1@_N6=5b>zsPT94IjqpwK_veCFMPk=gBLZ*Q{y?ZZ1c(e>`N8kEgp=WH@}9WSL=h= zxER=NlL(pD^l|B-94DPdb%(vnPHYSs&L zHODbl6lYe>(}ic9NL{dO5vw?-%_ew`IEWqK&vrcI?IrCPua`x@MW}$ z$Xd@EeGPXf_~JP(d&rQ^9nQre4NFy6;zz}%xJ7g$+vGi;T~$w|(=BGxJ%VJ^T=p9N zu~}@e`X=08rbXiNddc%S1YpuuV%)3A->g|J?BZNU)9)6ddp$wy_%u8Jpe8XKmgH)Q10KS@L!K^prMye6=E;?B|89w``=n)2`yTXGGY9 z$Me~hNW^SH%Gjpmaya(GB9bS)nT(vB3iGRuppa$8G;~@q*tad^|BDhQ`yLo!v%4uI zp|1;%9qgn!^7Xi8`%K!_WrbE+)gTXZY4-K91KafI4Ds5JnUm2NCi@-Zua=H<{7o60 zk-wTKeV7Ng_gAt_-_CP8NE*m2nE^FxY+%!LGi$s`mFdV7FpqH=w6EKXI*n~iqS(JN6724mbnv=g3>s%HB6HsrSmc>R z9SuOV&P5D-lGdRr%>``VBN<}be-v@ZkxZInM20Hbsg!;a6{0@;>{tZ7lDrQ;Di;yX zf}5aA;we?_y+o^@u7~-#0xCM!gvs_DWApoVFy%Gl+0$fGyt4WM-(Q%)_oW;{XJ0%< zf-6nPYP1ELaA-XKDIW|nrmHERu$0xTiNwEB7F*xctiY2RJs|d~CC<8(3_gvw>HjLi z;>Gv4?etFb6nfkKUt5@hP#vZiYlA<;syCU%a-$kad1pl9lOhH6M@6lB9N$w3F z@XCzH_DZ4R?xWns`>=LUnGU!hh<>gL`Rt@H7v3>%Zpm#lwA_qlH( zy|tA!chQi@#@d0T(Wvaf6q5f>6ZHSup;vS~*%9x6B*dyXAF~p!a_>eyIm#NDRLvmW zCyO|>d0AX;-5~e2WC==at0pU()X`A*KJ?d4f;1nlA%7?Mkf>DQw-m>rUD5sItyVo} z_CuWh?V3r?o8KmOw^cZ$o2po@E)2~)*-4JLH=?~eVo2Yd6g24fn9xI8skrzD@@VI2 zv|*MU6}&4&DMRnb`6CA4J2zXfKKKVW^j97>IX@t`trW?bg0XyIi8em*I~HtbK0 z1j!;e=F`qcC9WggYXyRsFJp+pnUvbPAq`R&Un(g5Y>bA#6(P1)j=2Ab6GUl!Ayc;H zl3-aOD;p(3X8h+tET5dOd51P4&)1=(NLGdAS6w2zf2X0$kJ_~J+fw@EX&ST{P;#uN z6Wx0fO`m&()gE?=$VuPYARby_h2htCBKCvH%<_%^pCF1}i{#+d9@5QZ@KjBuTgyg_eG8ssJ=@y82F&}+vBeA(9V zWa=jmq+&H1#om=dGgmL+Q-(j2W;stjXLvju>RiZcd`>6%?^hC^Ja>{YD;s4DH}O@f z;%wQec=ULg09~omAsahaLG=_&7kf$9)~R=)FHh{yf`yU%(Xs;Cq7X|qEodN-QrTQ- z*eYas${77@7pu8_dpz+MQzliC_t630SkyI2QgF}c;XFZBE?GMIDOaE+i8hVIk`C2j zvPI9Em}GZzhS;7OopdLjbKSXi!$rJpXg}$S8c&M?W|P{k87S}EE+G@JqWP!m(7`Y@ z>bjtuyD~8kv<-y+I~Ks~;$2*1r!AeHTa8jzIif=E9YP*3g(k~eQYXt^;(J3Ew8k_b z*IU{MNoOOA?tCQPcZ@n%sKJRdH^`yzFfxJjNA5HY4u+P%(o=3w8Lk2L3SG2!=OR#d zHm@CatN^n>a~j{V2u(D&DUdb$zJ))Y0vUOGcXcX1g}6{gVzaZpHC}J~&x>CU}5NQGZW9=x$qK{)p0eQ?2o-57&(7^iBWVEUpT$(hRYjlXErm2WV zoBbrCeC`m35(9c;vLTW%FeVMj<6!hdb$BcNl8222D6D_hyllnY=vTxdB(>KO9WZ~+ zsff)beV%hb4RXm1h0dDN0C#xsO%ASD)bNGAGT^E5glwBshGw=X(dlpVNJ57sU84-f_06LkG#H!$5i5pB24g54n# z$o;b1TKlKUZ2GIibYgZDT6=Q`w^QGh%1@h%%uY?E4>nDKre+DWCwLHQXL!OJFIy5m z-v_nm+R^}36TzXL4DR^25s($S8;Z(?(F?YQ99+5(LIdZ(63?Vv8dN*Vh2A@L4K-_gCdf>kUbM1@vM=kX+);IML;e}5J$)D@O9hhViaIK%R82+1 z)yea)acITjbaJ=+A$4?2pk!DVot4s|D#80}e&5N0iXZ8`TU#fub6$%q4iF<3{>2k# znh$e?F2aakS2`qK13ut?g3{%_AxRowtP_(Xx_LlZn= zBZK4iek7M>xYAhf^XQ6GI1=p!+BV>XdR=PKX<_%uv{eaKN?nGSLSK60R}?77z9R;X z8Fbv666%xGj6Up>hV=Iq^n&{oPCuqXU>+YvFSn0|`*y}`!&6y0)y{}Ev}aPE7CpL4 z>K3tbeGFd?dXXxx4BBmOLAPd`(}#X~ymHZ_OpS=vcZAP7ed-{Z+q(Jkap8EEgC1t?T6g8q0h77nJ?(HDbVd_$=`^-5R+WmZWj7s=B1Pc!NGAVV5{ z>lDmyoPege>4U1$1r%nQOIHM~=d3eSN!^?b>Jiw8Ry7HT0dG$f6b;aaaVaP>dIa%e zE~vRsp6>ClB$K5+lO`LXGEm|R-<{Y*yJ0(c>dZwY!j5OyH3HLTeB=IAW)k*s8p_Ta zB7!7CuzInB%Gq8(e&=3rsmYa(B zrC*Of<@Ec;qAvR-ps8j|rCXi&h&Pqs(tLy0>R*ILIesGnjiU5aVgu@avIybE5_&4K z9px?+p=w6b$a!5KT@@hQ#RvWe{@ZS$58bzjhG{Alu^GkMn&;7buN2UQw*y?v*xlrI z&Q_Fy5@C&dGr9B~v!mQZtnx+*wk36vrrrdUZmB>T>@tz6AhEV2TauYRdqb2v?P&c% zOOha1f@<|vkh-&3DDPhsjnsdQ;*>?eyug&|d~=6}MSoCDpbnENzC$-ZX$2yh$ro#` z0r+c&LxzS0dFP|R;ams}R(eAlANqjSyJ=wjtd}g2?xYk?c{fj;hn53!hL}_%ZH=suqEYt?*2096ix75nkQRp%ET> zVEVg|rhL<)JqfC4Xkam@KOn@dd?P4~%@r(N(NvS=U&R+BlvDfaSn4xxChxsVhQ@lS z!^W%s&}un(u&;Ya5PyWeY`Q|f2)W>rEittr+rFUAwHh!aCk1KBFSz#ur-{Xvd;F#C zTG+VRlnCYuXtm!kJtiy7Zr`Y;+t>YvY~0cWPtg~ynjxPb9!nrEL1LXAl&h zbdj7TXNbg+rSJvE!iGB?(B`rX6-i6b!iOF3_*oR>3E!3%>_4J^YpdwL>jO0H{ygOD zv5yJDP&%51oS6VZh$g{*qPS`Ep(F&)^Fs=8BLz%8sG(l`7HFNz#s}d6F!k;5ZcOVWDtbTLT zq{gts+Cy0F{TBSF=n~oa%AAc)`wSsf6$tB3-Ra;?J%YH81t)oL$Z8i$fSI=%{Pu#wl<;zp-t4QLxE}RK7%gp9^}hHFViQR1L5M9LHgs#KDwKKMi#6HMKe13 zz%42Y!i9#J&u7aZMsO6by{yF5PU?d4jp9rzd;+%Hd7s`i*+ALMe6rwmHd1(BPrUL~ z>6+5LRMOs-%icDHwe3F);dc$0vSSm?oN30+oe0EomO3=1JQ2OWe;53e{IRU-BpkA( z0;Yz~#0IYGXhf$r(coD>UrK4k#@-)?Xw2ryJ5xn5KqrK8HmV1Y iue(GCYG=_4p|Uu{Q|PYNIrD!kVyEv;&oocZS^oo@96$>I literal 0 HcmV?d00001 diff --git a/behaviors/custom/Dribble/dribble_R2.pkl b/behaviors/custom/Dribble/dribble_R2.pkl new file mode 100644 index 0000000000000000000000000000000000000000..9d9d61f444e95ed5bf3e18d57f12b4ad5cc48645 GIT binary patch literal 24148 zcmX_nc{EmE)W0E984{I5p-_YP zB15KBl1g(lN=oC;`@U=a&bsTab?-g*{;}^~XP?iog9ja(=PvQT!!2pTz!5#%&);|J zZqs#Jwy!t!_20bH+jIMN&)rGUifgv7U$@0?$Ik8k>vkq3CPmBq@5H2NIX|!eo06Ec zKq5S8w&dERLrF`LCM=L#0O3g+Wuj%gcJAJ~{(oMP`fqAd(SmSEFP_+=$Rp0D@`$?z{FT80?w=t`y>9+xboC}?w4svB zSNuZKi;hz9?8$U!!!Dvz^@j}ddn1w-Y#<9(tz~m`k1d=34pdX^;( zL}KHhjdK4J(2WZJKb#8|MkH-i9SG4v35l=f7pZRl5BBxBq}bLZ57N8NqJH!in9!V! z5_V-E-IzgFt$u)i3Rke^Z_DVvIg@z9!~~xHnX}R`KW1+f&(8g`qLw;ZeB4Smdb44W zP@4|rBVI+*COJpJjodsk+-5J_+B_D|$$qCfnn#)43=y|J;7qM&OeFU|=t(UQF_NE7@W6W^+b~W^R%muS6?$*2M?r^(4Sj!#svjDG##^3p ziB37RTN+8e=su@SX%6V{#v3>PI}8>xGZ5E(qthc~aeUoJ{{B`l@g3;~^xkQdS~!pY zdwmG{y&}-_vJR>}KL$42pAp5R96pRHh+kYBPYaI?!okFuzc2j2yX_R=sCR|nxkM!@ zJ^V$*DXXYNfdWgAGhuViFDFU5=h&}#u5`t;0v>hhHQ%|k5|)xgGIJsuJU{NMd)j7)3Z!E`mp6Zy_w}w9IY~dlTrjV9d&8OEdr}IoDAmR3Jx-)Y> z4LP91*B>b0ksSvysd_jQ556dP@%aghNDX5e=Icd)7oN}x%V^GK*+7um1?;tY0`o#U z@Sf&xxHIM`ea+NLNte<;8z+{X-MdTxTT*g$A@^*-p) z6LfNv0HVDdVRMfjcUxgiceYmI+aPaPlM?~=0%p;=XVZAP#%R2~DUvv7SVM)2vao8+ zUbeF80-e5KDU|IC=Mxh{nOHSg&>NS>?nwo(V`_yMGlDl zPf=qk%{p7d_&*sF6#h=BoG$;CM4TFfGM~0rDo0#`?^Dd+$G_L4RqGltGqWar*;z#T zt~3_z{>ht~C1F%dI7#^I0p`KCP`mmx8t=$}nl(n^FD*V?spuwcnH`SrqqOjE*EybY z&4MhSJrQ-1gy0x;4*brxQL7cFsP(IUnrJeOE*rd`-EBF|`(L>8SNGrG%TXs$=7qg@ z=mcG1?L8%|-k}WE{~n@zelAlw`vV?3-Q**lyyv~Q2|Z!60ZsB2;ObQcIPNV4C0R&MWJ z!!*+i!0z%;9Cf;eIHpcVzir<5U~dgiKBEP;XU3ysi3<(TI4>wFQ({S8cc}5*i2@ld zPPa-LA)HL&E1&-)g7GPWnm_rXC8i0)pkoa?`eQE|*6joBi7|Y$ANSC-FkVGc+@&kFHr$fb$)bX|Q*)z|^Z)baB*JQ1Tgtt#7Pw-{C`O`R)q8sQ-@- z(^}5gjQuT${PdEo8FQTKy-0%3EBC_MzxMFHEs=MQKh26KtVEmYBYcX{blkom5EarX zw|f-CbbegG+&S&AXPKN>@|Gc(%rF!7t-ObaLVt4awRJQ<LviQ?FvTXkp^}WqjuqYdT8agL$pVAYp$UXwlk2R_N7+zMQxM*f4w73kQS-nC*cA5yHw;RKVgD&om(iVQ zZIH)PUN%zofC7P$bv+%^=np>gYjEnn7nJ|4qY6hDdRdjhZHq8ASM?p+^(ty$-DzTm zZt9rIGch+KiPVlc0_jIP_^=<7QGVBH_ByB-KNrW*gXx_1CiJqqDk9=&JPu8WR3};;?qfC6vJxg*)`0s0bDk*iTHw3j1I25V z^sB^OnrSMDDbG8Jq-{I>Zl{XlgVlvMvg7#I%zIFunN2glUPaH3{;W^f%{L|1!$+fF zsLYju57o73wZm8Vz;y{U&40uTd;U?I&jzArj@ex6rz}jfpG&W89xS%MWr*_@T@h^e zxVeoSXHzUTAa|Hg7VZ_IkxM1^ynpsSw-zq&z(NnQ2g4!*B=|4JQvFKD7);t#SR zSH}r|jVPc#Z^jAFYpV;7Do;npZA#$Tna>{XuH*H^-+5iTAqMN+gMyYOdUi6$&O!Ph zoYsKeHTk@%as&LcImbQp-}85uiFe>FFvwf*~JZAbI5oumW78R~Wlbl$pS#uB`UFZBsmkA1*l4kbU ztjZAK-{Ik+w>K7w1Cy%hi}#uUbBcM8$cGj_FRbjcc#5+!OF{hQFg20Zg;=2s#Lkw2 z^pW|jWZ7jlCFFkPf35QwliLH%Io;N~8`k6Xf%?McyyCITyJ_qXGj`2R9hPK2r_Y}1 zSIn3dN>@$rgyWD!OJ(y|)tM9I%*_MjkAjG4zE@!<#m%(1`w_jiEfluc6mivg;k153 zG@kS8WA5>{>D$-`XyvPkv-UcQi|_B|Nny)qj;ki7{|v*IwkDXiBA8qdMAM%&9=P99 z4R*-wUsJTJnkL+FT1jYtD{=npsSx^SI><+cVdbX-7?)uI zbhtaV8AW1~brC7^Jx{ZPx&<tbtZPmjUBZR0n;|LR`nxjgeJ`GFM1T`{Epjl%=pRP0H{uhkdF7sndSukGU zFLRSiENAE5*Yo8BeAMC&Dy`Qp z5>4nP;-y33UFmXKk`WEnm*u$aNvO2A1Fltaw=SQ99P)pva3B z3F;1hC+}_^v7XuZiC9<{@|titI^Sj|xm;m_$0jWU&qJq}o#RPrFZF?xg*(&c(A(U% ze68b+tHQmDD26Z;O1C21`gqAPXT?E35y_VbfH4s(*jG5d^Q`SLeBDryLZ zNgW@^tr23f^)-H5U5z+!|F^~(>xYxNaM<}eyt7-I*xp~mI}SPXtMxS$ZLH}O(Jj*Z zU@WYdHH^ov_{7p~6w|9)JgG*i3`x9dfxd0c@b24Cm}YaD7Uj=jGw5*^^R<#d6@t-kw;!!tt_(qOsXSG;nYw=d&fjG`#QN@Xnx6HHZeLT${nhUYG#n?= zNB`+#b_Jm?70%L1e+9mE*gNhaUdJ7&4?i|^34c1(fyW*QXRC_k@cjH}=v&xGEM%QA zylnz3I^56K$p7J=C+f3np_MedUyw@Fks6K;k*t9;nTMNsBFtau-bhHikV*R-5?`%JD*qcLq)BE$)1_iy`=!wZgfHKY9)Si zW(@z_;)6?zhKL(aXz=x!4a}{362y48v+(<)nNEd1HH^9r&Qo$=iN`CH_o}5+(huVE z+nag9m?&_4QzUTM+s{qnHLqHCxW;IAy-{%1T zm0ac02flK9^#tfFU&}QcMvw{bpAb_uCAvk=k4!y2iXMC0##G}3_|)4C=U&vnFO$>w z^PXgg6z9|S789CaYKc;R6ouRG@1wH{e$s_!_R>g`Yy57CE}l6UVeK*6Rvy1d35ywFtQ{#IOq%%xZ7aUxUXC%0paK&Ihb60oVtH0!wOw@Bz_tw zap^g?8!3&)%@%SG)1x>qRSHW#y5euc8L&$1WNm%F0L-8Iz$495xV7Xc{_^YxB|#l| z^FD;Br(2SHy0f{?y=h$jiw0czq7L()ujEZpKS}3;D|}ebNS-oIk1y(a%mSLr*n!Fg z{L~5=xaB<+ZPKUX=Oahq#;<#V>)8|}zgWR!)Fc0;Wz(SMdALEOiYKRW{xL%bC7L7o z-t}$tLqi6VwK7g}l==9hdeGhmJ=*Ftgzwgge}$tA?oHgI7Oj ztl4dTM5t8N&5Iy;WHGz{si+5U&F)artqy_#pIaOd45^EnI~^H6|ei1#`n0& z@rHmd!N>ESRD1q8-mW*FYd#nZO4G}5pPveUIZ2zo`+bV9oAZI>J-i8tW3SQ0F-u5X z(m{T!uK?Y0&r%I#GwydphQEEB$X@-E=UzA&k3BpLJ_;H1IG+OXRr|Q=qPILTcN71X zI2`vMx`TlNpfA-8xz=nMx>cbX?EgIw1YsAi?VLbEjH^YDT{nml)wJnWy znM)EejbE6+cvH$%oLUr*B3l6@*zUrL&K_LGET}+#Fm7qCM78*%sMy^|=j4r{zfyj~ zue=p_q+t<}oMOc1zHX)$FRIY)U;ePGwFAG>LzG#h@U@Q1aPW>y{N_0X;uH4JA^u8W zH@*g)eLm9i{eSS(>TxtHPZE2g|_`9cW^s_Gd|d6NcQ^oiPkt=70mh^LuP9oqYs0U z0leCHtz-rI8ENwy9>Wpp#h|FsN+q5q;<}wR;A(Uj^ z;zf^D6q(cg+dO_w6Mwn?5G>Soq%Rx)u)1}B`1iTNuqS^nG`6{m&8qg}md+vK=N~Tc zl^afwTfSi+b9)c$6Wu3IE3yP-tz-D=*M`{k@)x`fR~K)&BIJgZ=cub^JGK8Phc`~S ziLUquk)w0+G4Zbmtz(k;C$CoQaWWDHI7+~-E$?W_6&Lt8N12;meIQU4o#d@RZ z)W55tC&7|l^Nxf=iiHqyWgdUM$O}|vuP1FU&og@chrq?`mdLvAE*qI>3=?I0*bJi( zI>srT+L`{xoJ~gpUDpY1Ucvay_YO|^-A5ms8d!5Il!g1w-GWB{iFB#d3X#&TX%Ol< zO#H>{FZOo*=FJBzshRYC{wh-d>NQbNVY`iPP?N<+?XN++`Z&rD;QEL6!sx_rkua0U z(oObWkT0(Zp@#=i_lK|evDcM6a`;AETp5lkPwe35)=41$;}w2<7Dzwjj)h*kCPsuK zQ7JQ!JuEoE_de9c>HiheD;_bdNm@bBHSrjCI)0H9Y@EbA?wgZ@8f`(jX#<^c&V`0_P3>B=zwQsL~F>P5OBd8?YD^rsqM}%V2)!Mj{TPuxc@pFl1@%$4iSoDHiFJ>jvTAMt9BHfN5LD66y6ZRLQ%R zTsm?a=Skk z6X50*4UH!b;ZuWVh_X@tzeowZ7(mDh9pmzj}uRKSOId0$_dJ30HPXvXGRj_{g9EiSM zLeekjqDAvEG|I3P!NH@{(@z1CBPM{0eIY&P(@z6q)Nsw!NdCfbDb+e-Nf%EKr+6;J?(8xTm2%T!_`8zU&}9Iet9dYZ;2G8WvFbLyItDavGQfe`9}2-?2kl z$~?lA@S9e9K`o(#j;`gL>sIo!AJ0Qz`Zt=^kN~>ljKx3ameDV#M{%2ER*P=BF|7*Kg*3T&n8fCzYW{O zJF#M75cocu#1HxTkn5=up#M_`oo{SqrVbN@cV>(w=YQ|x%YRR@4*uoNT&K$Mz@-vg z$w-}hfSCR2HWvi#Uj_&F*IWNE)I}pk0m?(A$5zriOVg0SgLxG1f?G0wyCG6gN_mmEj7fs z&Pw2sS0J8vqD0)&HHJ$lp26|4=ipi4J+9o8!yWEv@DU~zaHdEEx6jNG9|$+buKhEx z@cB0`TXjt^=9)gt*$_eV25^mrNix2_=m2qnf1zeWIGD{m3J+Irrn=>Me0gCTD8A{V zt0xqJ!_D0rxbxL0@d#h0m;?(gMk`;84>m ziodD`4aHG>PL}|lw|%CbtDjTm;k0&9`Rj6_p)4X!U3K+w>bN@-3lcmZY%SR9AFAWG{_rc|-dws_C$4 zH)zY)Ei6MRg_!+1$(dZENFibk44)au2Wg+7zcwf_HT{Ep-Bf3K$bCEN_Y%10R0JBc z(jh(b77ZMK1oG_KdD^LBT3gTzMSG;F=*|gLIrftd-J(NRZR_SH!f^OA^(yy1H3z5f za)bNZa;PXnhr3?V!RAH5WVLQRAG9@$UD{E>J*so~f|588`Bw7v3*uRAVh6KcVao&^ zXPLgU9B4_|(fbYIAl>kp9eH5HH$O^B*SC~tcU)vj;qjvTGJqA!^_j=8Ebg#- z58qK^i#~fy*?dz~k>}`@0?R|PXff**-}AN=rS1 zuOJ1(6ELW77PuO};4gIVa4<5bKKHEohx6W4^6@V8U!Osxoz_#8E1GnWc_r`pmBn|5 z3`5OdiG0ejTDdc;z>k-%?8FuN+44+GkDVIk)9P?-Q`@ z)+aiz%Y&adRY0SLdV=rX1Nd;qWZ0j%61N#@f=FjNNH)r1-yL)C9vhEMRfl+A|8YTQ zsx6;p{faaU{X+llYUV#5M&qTPKQwHMKXI#H$ip_Rfs+O&xXS5u%=l@$=wZt$R$L0+ED=2tH@}{&f zy75j3T_Omh>;HDMJGwvEq!qcOb=(Sk>X-vRr%B_)1-W#%#~r+H^F&a-ZV#0{Da~E1 zGP!YGA>F5)iZhdW54P9=sr$?Yrot3B z1ApE>fC{zA;%?sxw0KrZBW6t?!e@7AtS}5xsFtuIa4dZ)xsO`tJ>x^dmGG;|6LQu@ zfz>*%!v6P)!VoJ7e7W6F_}6MUeuy4RlcU>3ewWO_eD*NNxTDJL^_Roz)(~K#0`B}# zfFs2DOiy(mzTG1U8LKSBIhyl@?N>g~d)@Cz<*!H#=?*64E|xg=V=%@{X$6;K0bYtFyCSvA6h2Gh89j_2I-6Fl_3eDg>x-QT*_p@#2`7V^RJXK39$#@89j&4c6!eJKh#s!WzYTB*}}nF<>a_jI#_qgf_wcMuzK1~48jT{~cFSwhJ1wWsF@;8W;L~qW z=AQ;uvT+1$B388b-V6F5dI5Yte48YG+QY+hCgPsmUF`l|C06AUSJAiaEHnO;gS$k= zcx1N=y;Rjfwf1%KlGtI)pmqwzw^@rF=AINLs7q2)6LT~kH(t1Yzp*gTr9iZ6&pGCF z^e}90F$P!nx2QGA4sHg1!ip|}8XhmvJ#rCSp_0V4EN4J~g}Hd5)@X2-k^#AnRGMET z#YFujc=5RaJ;n}(J@>ZKWp5&&WA`z5EqhON+*L}TwWS8Ko#sN&{0{23@i4FPjO0ek zRcU?AF}l0VhgU_s#y8#_yi72QYz*-feU%^Bfx5ZUQ5!6Iy}AiMzHU5M2)@NG-0%lo zxfYY>Ac`J8QTQ*Vatt{D%V$9lQ_bfA^+m zHFtuFcn|&0^*Aj}Ze?D5r>m(#O6YJNwlWI_ z&$~rbZ1sp&R6b~Zm%?)`KJZ@R1Q-0O;5Reh@u5{W=_ls{ymGd+AmElXU$*!OIiF@m zxAJrnA}LGXMg|Cc?DgPQKoHNKrCqr+O`U&F0xog0if6B?=H{#SV%kb;@bMx1;o6~4 zTiZ&8eSZpOYZt?ev3pVV^n0G*;Kt{#ZlWf&(X{rbEC78uHPUzBpSNohP+C5L~!A&UiRmi)=ut!%4>lxW$AV|0q-FK(9}PklV*lG=0b ztgthb55E^oZyCsw`-iewOR51^G=D@NE!fS*lwBeU+hPPu>WX++b~;~w@*~JD9Yho6 z&Jcg7J_@?_hOqNXJAdQ!fFxF4rduby6PF!aL^md%2zr1n_-92Op9cdWCQ{PSG z?l1ol_stYssx#@EOciihQ7y8oRi>Lnisbr5kx24}KA&}dG||nNild>12TmureAytb z!Q`p3n-LnXuf@F+w5ZCdNHA%7fTe4bVAi2%P@1<0j;lrB(IN5Fxit&HDwbL(eiZci zE72uuVrWmRGAhkG0A{8RRIBw6J^#;)2eb{Lzw?y%{fZ&nsi1{bpSI=JJd?%;>hnuB zTJY-rU_M=4633=3q)}#_?02Y$vnnxd-@Jp@mP$a#w4F42Qkv*P;4xYy5k%~4i&$&D zB%d6$o-Myq#QK^`*=K{j(60On_ciaMiJ8y%n|I|bu&WCFZb#A~x>aE4Gvm4-CYQ|J z?M2mf*Kp&xKe<&!!FfrgqHy<_5lFY;vh zvuo+#zG647EKR1Vjt<;A=REBZETd+w`}un9WR@X$0yj2)rxUIjaDDUZyuGZK1kFj| za~4JO>#w(Ru5^&A-`3%045N7NjH7h5)F4=7?aXFmy=4idA#|tFz_pDHpPs&#>BQKu zPo8Gr6sAO{9}~i)sxw?}=qGlvdNwcpbz%obVxOJ!0EU2)+qY1a4DhGXw= z(9S6&QfbjBJvtaRPpSpwnJdL+$J^;*+w*L);w-WH0T3JQZ6poRtH|MH3+TO#)j07) zD(qa_!2`XtK<>T~-&AW&=XUzh{Y|sbur(S-ts0AmHS0)rpgOkqR>G$C09@kzA3B}a zM77OcsMu488doOb$bELS;#vlKx_&UY6<5;R_8)PMqAl&6x18RYm&h%%bE(bj5%9ie zJy#s?m~CA78}hnF^G*4glAt`Mjy_$o7p6`ekD9CV z>Bv7P;Xq&o_i_8ghPt@HNODIcOuivl)B2H}9ejm49vF|eZb*Z|!eVkQu7)4~e3`{f z>SJv~Pp~K3&6vY=Lu4kyKrW?&OJ0+Jh2iJ;@$LFxW7xoz=JueO^HsFynN4a2Yv740 zMRDQ#XE^@;F}RsOoaFu60P`0o)1L41X?#m9SCQ6$kVUby_;m|8#!mAOX-RBYzXW`| znn@?MXXAe&6`Z%0(>A$QDpe7}PYeC{_|7YsAN`4nq(^c48GliyUW!Ty>>)O4KRQi_ zhyMyWs}7#bZ8kbXMnpGvTOEc^Q>TE(aYq=Exe*UZE}$7Br0MC35PbS%3N$J_XUR#W z?1iQ)?{=QTy*GYhPdZg`YySiGq{o$vC=llgM*f2yd!Zfl2oh`P?6agu?#AU^nzA$hY}n%MM$}*ck@wd@(rnzOdFi z--^i_M?lB(r_^xzYFwwE3o*Lpcxv)o@Sd+L>@7c!&wAuBRq+!YtKY!S`kM`Kp%w7C zBm%cbHdT&(*~j?sR34{h2-dS&L`DniS>2K_fhJ_IP^BYO=bsQXkA6dy>6bv~z;P1x zs*9CLjG{4a!{C=}BlACOO%)w)LaePfI@#&5$i=64*vW_d(d8r5d(KXBagZ+IdvsZc zd|9R5bORdcv77sT{Uf+4eGng(enyd{6!t&WLP2tb`0Of*yBpF(p%L?N@$YYZsn;DI z*r0+Y(_fLLG#y_o>|}q8>Uoj<5LUKq0(fq&C(CZc;gQ;Q^lQz>>g3aO#G^#M_T(0@ zv8HS(G)9#23l)G;yb0y5b^K`ypY!51@`y(tKEtuQmTu8Esutq zm^px{qqxF&CA#SK5#BiBD&MAdf=}_@#FvIB@P?~Vq^vsyH(hOHGNaFvo3AxUQ_@GW z{N*tEI6x6(i&l}WK}zso#a(*N8u{w=GtiHy;=_4g1n)gV`C0yeRV+%zBYx)8`HwM< zYRI5cukPS0g;8`F%n+a<_{0gs@$e-ruJI1XD5ej#_ok+9%HI#wxsM6Th6tG$l$vExp# z`1#wY?zFpHQ=CcP%}wOj?{1`j4z5Mt0leoT8xP|i3Bb~(n10>45d%j^5Svj^kgGWv zOBZ|)wC>%C69dKAwfH4Cz0-jn)oLivxqux!iUmzN!pE6~@wjXO80>kB9+%^=ZpAsC zoMXt;r@kQDQ(x0r|B6ZN^mVj7Vjlfv&`jcEBZ+7D4SZaYCde66Tq&Mf#H-I8fcklh z>7mRv?ymEhOdeOrCpjC40!GNtvnh;wtdm2hF}kqo;7L)%;wxnO&`fgGzf*91&3Lx+ z;%U@YUek*X1+awSC6RT zCm7I%*6VU>&nb=@Z4_r)_xJq!ZqQD za+D~sB!>UI^@PURO<~gStKis6C2>{0B7Iz)%*OV0@awx%>D{IAbk&k`^knsM>MwbQ zTJ^7o5W_+cJCB9_CS~~5U&o8CUV}!xe7e6wpWU`BqRX0^Xu_r_+!yx<-+v6}ZMO#r z^`~w?>w_Ec%_$iMYsy6OccZ!e;^ll|@hFOdi-Hx~8>#c23ekqb9JqLW6Q)Wy!BcOvL8f2`T$)R??8R>?sDJSGN_0&rmHlL za^2foap{c+ut`j&t9t$DxD`rN)H9W8_RqpmndWd}N&*CX>Ef?4S=bVAo-E#}2mMQO zsjpiGKJyI0`yDU&tE@)4#9#y+I@TGd6%^u|6LP3f+RmO!e`V{ezVUy5N8qS!r^uQb zeJnq>5yCSM;9=h?Zer_>F8K|_FMXs)yvb5L!fq;`{oqHwCq_gf1+Vqk^7YP$>q)~qdvkfB`)As#h3Kp+f zujyra=b)Zowcr;0uipR-;O6E+pVH1lj?(tF%$aP%wks^^=){dXZ5_g4)OzlF`!0yJR^W3?iNAF0 z#1-R4;PKyQxz?kZc%fxB8b4dcG|Ns>^IA`utqrhX;C~sHFXzR3x6_*|BSBtw5ZBWv z;0OM+GVM`?*6$|!55#5xlkboa$DO1QtbYR5{)$E8;!~tPb`=pGJcIxKjspYx01R50 z$$#&ENXGm)&Fl?g(XKs+{5>d-KhpJ4C-OEQ7oiS|7p;LM+rDw_D_JxzB#j0|%!CQc z7}b#uz!E7pSaYn4^ytl^*YppNe#U8#YccSk~*C;?>`<&)D7^SSx^Qat%z3Z7Kc zC39jP(Qmh#sPDpO*39G`X_NZIMUP+6!X`CQ(CZ@l_^1QTz3@iR#@-3WYyB48-!CH` zd)AeBr&^G-+|`ioQ%>RSIoLhcTD<UGzZ|s;B)3I@lWH!tv~^%#w~i7i z+x!u!ZqX3$Rb2sN`={`9*#z7$E00?aze^&E+@N0F5=N-4#8tlzlNXVh(0IEOEIuEl ztxaikLc$`2y1f4+x%S;;h8^DKS&=@TCNdx8GCJc^yZ9>y?4f;}>nv!3!k z4?n5}!b5*O3@%Q>Ps?_23xzQVr_S;G=m@y9(3xAjJWA$w4S^+rXL!+PNB*!h9;W&o z!%Zo3pmonLfj*oSsPHbb^x|UbFE5V@Zwjb#X(fMAnF~wajE0N<5j<;OgUV0C!2Zx1 zE1P2meBXwA`tzGP`d&=}$=3-qGD3>(>-<(ZPV+K$oHIp*(0rV<%?J9-N6_I$zeNf0 zJMily1^oME6twsTa)bTP$darZ{KBYRFv5Hms+T!0^Qpt%l0o%5D_c#9dE3;*+t?phz+@~-lPAdY=Fe~WGVC?~!= zHyVQ89>dg*EHE>61LM0g@FQoi@P6zH3~&sGQNc}>0kdRiOU+(hA5}%9zs^JR7h}j> zm&yS?_BcrCjDf}iU3$;$A`O#^7L~nu$aOE=AzwE#QPwFfP^(`Bmm?O@+a3-0sV|P- z%8zEUU3KL<^K(3YbtDf_OMz7_AGpzpn#w~#ouV^$jadA|HB6;6 z34BK?!v0swU|hjlXj9G>WPY2DfAmyv(XhMppu-w+>UG-y=3a@hUq@s2URNA-FA}~y zy^nSyS8~@2^~82oJoy>y#^XNt@UJ??{Opa3r2gwvUh!cG>{dOFS6w^!54B#fDHRHj z|C@nL|JBpdpuzAxR8t(k=L|MG5vo{SVBJ(RzPEpjAJlVODb-c@(o5f6T}PovagB z@H7N+z4h60w_wa0Qi}f>sBvT32M_TfiyM7{C$=bo>8_=$EIWqYsXq^nQu6fbIx)L- zx*FpRljx44MPyNYIH|a4N1SgTC2L0=5&YiV%nK?HQ+M+*+-Yh)UAW%_#%Sg+rITyH z|J_5LzMGMPJ7Ki>rY@A7^kE)Tqu7u}Lu`3^jy6XmbJc-fg0AQm7QHK&9S$Dg^Q5lO z%^|)#|B(sO!$aul!*%dw;vq<#6at$E`lEVXjp&d;gon~%T&&VP;QeC!?+IUKYm*LX z-P%GGD_vNi`xMs5Wbp0bO59sNhG*V30HJU%udXwLS60*U!0Se?eBd`Wg;r7bHw|bm zV+K(Tcd#zJAHx?a^IhKG;j!m_u&+pGqJ(23Nj)-j zok1`?^gfGMlqB=4%LhsH?jFH${EYtHIef{OFWCM#8SA7naZgJXPftyQ_kF>1+HP4; zQhJQho*QBR6EFO5HW(&ot-{Usr^B@zV_5fYKh2%}88l0}aBZLzU1O7g*S;WisWYc5 zRWh)lGX#%JPlNu44FV^rzoMn`O(L_RVce=CR9;L;b|z3eNerF%sSU&^ze2K8LM=s0(>4W$LmQRLvX5j1q@ zTS0Ebbm5pW|3LNd8@l^+Ayv(i5#Cw&n0;E_PVPtlSlF5Ike9WHH{KZyi;o9E_Ja3#Sz3{YG+d(L z3*U1yyNP6ze-{3Uxyt+(u7nLcn)tcvru<#0wD9YIPM^0XgsS|U0y5?*`0SZBbL%=I z>UepDPOA~))|?djB5fl7ajuPr^qHZtMFiXSxPmU0jiRpOGHB1b3L03EM=xjCagVuc zY1Il_46W~>0ZZM%X4o2ZvAijoeB%*!F0UY$$C!cR@}Df(Wdk>m8OE+0d`<%@wZXvF zU9_|Gg{a?@(GPby?JCaZBmIqqZ3?;k^*|3!XFxm?XdT2|Caa*PHXZ9(}o?9toqk5sM?cBui5IL-n#FqFpX7Yo5t={Ki0Xu!519RY;BN6$ zaB_T$%44I!@4g&-*$CuU<3uWQw&QxG8+lK>GHzdT7-SaA|HHXkmk3Rn2i6bZO9lpXCbFzFCjLkWi{SLVzZVg;2RFhkX1UgiqDRV{1Vu z>RKveTFiid(sp`fiIMn+7twwX4^b9ztH1TX9MaJi$3uqjPQg5CrQ zY+NUap9RduzoFyBzYj|bZmZuBY@e@=w|CTAUy1R;OMBW#|Ctna(nwo8?7N=u!s{v) z7aao0t`we(@8sz#ifMbOEnlX3o|K4#aIA$oJ#pv+T>M&zEvp}*uih`17GVSZ?@z(I zwRJGOVjLYJ8UcN!^YP_|5&Y50H}pxyK<#y$LS<#5`GgZQV8!`d-dlBq7vzQ0mKaOA zA=?nw-nhe$!|y*A{##8kQxBskrhP`N3rN95(LPI?o9N|L88xytzCmLq<=QO`HFK4yb8my%9x4Gb;Whew{&vpJvp zm_w2U8yU8km|Zx7Ep0WJ@16y7Pvv6hc~iVQqKDS@M8m#$o%G{15AmFF64btDAo=c2 zgX;siGSVQMcNjPg^#09)l`@?F++zR__Yj;lc{mjf{mFlK454y6EBUCLE|N8W4s6*W zfT7P?(V*H>e163|Vg8JtbjFL*H2Aa%SA6D#HbVw9>}Gkf&W6LZ_`3}~ytR&cT}mWI z_eY4^+%H1=Pko;4oXr;x_|l8M7IN_yC%QrV4L@*9S7@XY$W3oW^Y4rI!}gILEL1FD z@;Ou4A3Zzvye*JO`^KRQP{VP5i_%DRe3ffXf41%I)vgShl&G z9^O+*n~jCk|L+>Cb?%3(Z{=9AX=3H8l023kFq_Zla_4Gqgd|1X0O!xsgQimx@zn7% zQ1{c8@0f9i8%VyO-`*@24*$BAhU_~f8lHTR2E39%xsHB;we&D@amEvz^3MjWnjTPB zyMC&$@3`H@7=?gEob3nVLet}yFr&deoXw*JOmTZ zPbMp`kD+oF!O)5aVBBn1Jnb(+my1pORZS`czSTkBOS=EBjWd6z>J8gBg+3ISry`@FJ z`~C~-IQCxazOM5+>5!E@sBv3|Xwc{Y&C@-oRpAj~anl{y@5K$2PTu4DPlWR)yN~iM zz2Cq}qdHo+Xcicadddxzw(z&-F5&I=57-%OzHD0wPuv#B z6^cev$Ez(|IiZ-kG|r}1`8~96J@GH@ zy*HLqp8^QEb^$llC-4Ve$Jm8qK6FxAZyr3QKkGZMKli+v%;OV{XxjU3Nym2wmJv9C z-8oP#xH)H$t+MW|=c$==Z69lX-tRg@1f}x5A0OeBhhx#;E<*=zPuI$%n;1T~lQo?A zfj8W&X!on;)7Y_NT27p>HJ>4SdasUx&qSGg0%ZfBYD~QTDWU6m)_d) zokhil(P#rl-tZ%njyJo&19cAO%D|$c3pE^SGf|O~pMh|+ccs%!P=wPkE zZ^*oT0X$9Lo?a_@4~IVt<-g7kMen3MwElh{RzCQR#ugs<%UN0aSMCs-(3SwrVeR4- zn;Ow{xPhm__TP)+8pnZx&QD0y zwx-W~`q7d(OSzi!TGTCD4*#N>asSo_aPG%#ej?Tpi>Ga+LDpyBU3xH|&~<~W?EcK< zruT-|UfsNJ_j9oP(2FX0XW@qxjZ|*Lc5JinixG7_X=9%-GQRi&d;TelUFtT5%R$%q z)UHLssLB68dOHG=`o7>ZW`)z#1 zLaoZ!OI=Ehmky+N-|gV(PCfZdyF_lQ=fl@`eV|hpbkP+Yrem9ONSzi{kO|yiwtXda)|59OB zIs?;?{rF0$2$akx5ps4aw3u~~K8^_(=bVb)Y)oN9*mkC|R*5c3-U~X{b~2rp(|FLG zT0Xa73)YTNhuxJSP~Wr{n9Bp80aiRx97?ywmOyK^AzYjE9&^if;JX#N7~P_TV-~36 zEL9WK*V%?|@i4m-?t;CJ4+59Fo0xWO2DALV1hrJ=kR?4-$m5}9tZ;G_ndqrRA~f?D zxp9JMH6CK8W{wx^Ml_S^6{W%+!{4H3{B*W+LmzrEAq*xoe8;;b^7KOWLdla1W3F;o z0e1KM?yC8;xMKQn2P*&c7qxgD3kms4n9i`fLcM+~Ir%n^PRi^{C%rs`mv(UaRK=Oh zPpJV2SE0-H$nJ3WYw#jz1?FE|A>)iXsn*NcczJd^tqnTBe_A|-n$N+s(fuMX9`l7d zUB5%jXMCV?)*8^X^f(RQKY?G{ybbz*wrj3%n#U!M<^LJ{hCcOj)NWQ7TmA4kR%cgZ zyZ#zD;T;P`{yi}!$d6Wr)=M6qye+J^en)&Qn@C>Sdu(x+@Wp{qY^C}zz8(tUz}VjW zG!TgWwvfNbv!*kroALFF3~6L8BMS|}Xv>S2WR2ApX^~|Hz6vp8ll#9H)uw-DQGcA6 zn*Iq|{XiQhm}Zcw)stxNP&L^2S;i$lnn!m3v8w3%xePKru9M|+NAZHV@tkb`fR{u3 zVc(3u#H&@x(`-Zd+l#$j<-ZkR&3y;3EQrIIaRGF=Pd8J!uZTK_3Ss~3N;oOz&~-@? zNyVtq(EL=^U(Y`SAFoEhjOLFRVy*>EvU_XP>DQQlsW$|*w&4iJ8?4||2HKw;&kI8} zXu-K6c4v7G7N-Fur<_ISL+W&o?a1=epS*y`zR^CnY4C*5MhsEAOJ;bPz~+D`7=6hc zOcsvh-|iaV{VA%b_4_FA`!Ik`J^Tpo2-9iWatpfGb11LqlZDOe4q??{OTO5u1h&4B zqUnn#udAc{E@2|weT$L3^2e~Syj|p z+DY{<9&nv1q(WmwHS{uyrd@lN;iHyU@N@VHRQ1%W$X2;V<(jV19!D!NVXM97Y1CET>;ibL_#4NOoG#qaf3UdmuYDxxId6-LU)|Ha# zb@8CCAI={Jn$XJevR*=eIL2y)quXvh%+_cZr_GWjet%Dar|&4ZlCh87fIXnKVj$P( zj*zw6o8XAQly_QN@{_~o!m1skuupEH?7zsBXO#}8?V8yvtLO*o@QRReUp=v+A{4hK zwy;ONdU5q1t*AsD;I?-oO}(1~?Z(0U{q95_e655`dLrYT&m86ztNQXFOJA6#X9dw| zX3QqL8A3X5(Cj_o^nA=48hR-o%~Bu3>lKx-CU`UUne-g4Ie6fStQcxnEc+(58s+Nn zat=SQ%;2=0G5?hHmD}vy%-cuT;fdYTA?eIl{OZqPz^yz`wNs_5`kaz(Xej4TN6zP; zRov*puIV({D;u3HOt|5=k65m*L-|u3@GNzg<(8L#j4*6DYXboBU5e89|HINVPeCC|@-W^OlHeOtnN=E%9)lA+W#Fp`GZ zQ0KRUcR{EcLaQuW9Z=ne9nd#~h{Fv$rt3X(Qu`zEvX$+l6ct^qc0@tl>CwFJt4O-S zx?30?5J%@$$oQ3y*|br3ChKW3c=zot(&M)+s#ylogVH+K(Ctmb|Hkp-t_9Td^DVLw zbJ5x~5I@_k#z}1@_~pV;iU-Ey3iWPupEHtT$6nn1dmg!5aSHl1{}z%Og9NpvJy)K%m>b(in*nCb0?qG> zm~t$a4r)l@JN;E%BmKL%hi(oXvT8GFi@1ZcVk~G_S&6-lj^7PF=upRBW+uULpz0seR`zY`a^((~yau63C+=C7t18JuN zkU4TYK_TJ_SBMUUUoIu2v(kgN&PZcR-b{k>u*GmYe?2&8N5BFyvl}h>RO>m~UShu`^{|cIV#_2m>FQw-(A;An4ULrNa!ZQfjphfe5%z&{ zXB;eww{Q*B)W9g~JpRGGjI2Iii7A#h@%3<9elGcxjE~QwAs)+E(SId$g!O;C)M71h z`&cB~-W=x_AD=t2+)p68${B(M+cMF(4 z+QI#PIMcsZ1=kK^71z!+N^sO|I9)F=IPk=p|9?f;c7^DPtoW!|>`Z3|PEslzl< z#?S?sQnEuKoIYI*z?4o-aIGXhDW3 zJtSr8ehB{F@#4F~U)ZfReZ;Ex?LubDEAj963bFBqhcL{e0<~YY^56(0j`~l3N%-k{?LOUky3Y(LR<0-Y*q*xLhH3 z_Be~L4@NKt?XzS-#5>{ix-$~KK7nl*6iiH8QX~gQ?j?T$ri-mZvWSLvUl(QbZxYQj zN5s_WaYT3HCt=g`y{yTrrx@Gcm(1=M3AO1bn8wUbamlw8;zHS`?{b;~u3dkC7zb(6 zsoy=oZ+ATV*Ih|WAIU+WR~2b(*um)AMHqKM6Mx4|5!V0f5CeOpv-OMAainWc&^nPp zj5?nRD<}Jr-l8k(JXWQo+w-?Fo^&g~+aS;C$ zC_?_-CoE9^I(hDJo7@an0FCgacrSYto7rrIHe1Ts?K}01+FWE8&YgG3Q&AC`mR%5o zI^vk_?Jvkai7fc8Hz|;1s!IwF;h6k;K-w4 z?--W!Z9F-w8ctRYRwE(HGRYgcjjZpkA#kL`7Q#Fl#fKqBp;JzoR1P*I{d4AsR>ud> zQ|*Rqn#6~U&G;b9Yp9cqezJ^J80nz>pRMB5mA%oz zB@{0mh?95<+USOn?^&No<%rL0td0e9e`q5Hs0B!}ewC9t`2=U9Dab5r){vB<_hS8m zEz*d7s{v*ONTbSiAta&_5~vBOjkJ<1cK2kj7le^!O;PH7ErPuMnoQF4c1sL~U1nm^ zE^)SOM-w{tnQ+)MiA8)qCGPsvDb5i3Lge-YY1o}&j7n64X>%43WAkB5Bg_JdzI|aT zujY`d>l?@cF_|pBdRxp*vcN&f%J_8DW#;(i8)-;CL@ph>EuLMRC6r`4kvd5#%dOl@ z?$^H+R(MijK}eN2-^qg+?b0NgGYGjpYdf(VKA$Pw*dlIP-Xx8#9*i++pUW-72a)xI zG?}-L7nv@xLdTdd?Dr}=*1D|(eko2y)BI^du}2;|(q%`^e(;1n5`S_!$eUQ$1+lYU zF8JEfm^8VMfK>h;2^eRE!L`wuZ{FJoigi^&<@{)vr|yJ7d6}$pNTRrL z|0pOm>nHr${FRw$B@i#QePUVHFVc3SfWZb;sa=I5YkxLQ#_2p4Y;x_{tgm)3=hJo; z{7VmtsOeQ4Fbm^FLj@0#0hPjJx zLHW5zFtH6LA8*T?1aA|Wn|=xwY(CB2tv8U}XZh^fM>S?*VkN@Zv!X$F6_ZT);bI%p zpOp_&5Hn-mvCs1~F)aevzjj|XH7bL>N*e%0u8A0(exIzLx=rxwZ3_vHaxqqJJ7k_s zV+uEi3)XOO66o}Yk@Zz(*035r|HJ_ts91V9wuVbjy>!t(Lg)-N#xhI zDz?^5U{jT*gVbG3c&!%$yOs6WmJs3)iMqMN`3d-^I{>Q}QODua`k?qT+t$NK*k;h+x=O#_DNI^IgQRC-jFnIo}i6p z?4`*A$P8>_5B&R!e@pf7Og~jD)^W$w`6aBzZ#lZDW-x_pE9SOKfyDjI#t|XIu`^r^ z-yN@HujcikYv>42Q5i?3_zhrNzeFJ(%#mq6tAs-mYiJ70#*5btFvT!HxN_GSVCk zQy(OO{*ex}yLJt^k7xC{B8x-sI|9dgg<16l7g8mB#9 z$8`6s!(CcFOk(bbZ7Jhn==*db)54u(4o;Cd4tGoSFH7O!v~04?r;|u)6iN9yEqr6& zCXp|{B`U4V!MVy|$Cm2n#3_$*pZrTQwT)xEz!Y>JjH+qG|*S zwM&`z@7JI=<{KO3?kLOOH$%md?c(#YV{F=XMSS#Vo^YpV3)D^ag0eX>SA?vS7`Ofu zPK(_swNF0;?>i1bnv4=t?Eb~_W@@0XVgbp1YKUu{lz7RG88|68n;n^E18NVe*=zM% zqSx5luzNaX+AIY1Y(qeOv=3O-CqdrEPq=?Z9vm@|v7>68>_O3E<}=_TaW6{(cbRj_ z_vjXOVdM|^FfK`!Yw3_$yT9VWB4m#ro3hjF2D^|n3f**^VEL?dpsSuF?kkgdcD(Ii z{}?xr)(wRpE;rG2SPZUtrp(U`@MnLo29Yo2fLlWzk#OgBHlTDT%zH2r)z;?olz=`o zhJGg_@(jqIvLced{4i?u(FCUryD)pM1%H;f0{6)7v67-HHsB_*J_@~wyZUGbLf4{ zXtV{xdFNTp!AuDFVn{v~RSWIK1>onQj>&J_S;K-0*fuSZr5-2zMbsbiZ^>05V&qnQ zy+sRuCKbZf`!+(^xDzZVq(M-SUx3;@9tnq|(}=LXfbEQxJ-^RkU>%gquEalNk(2JB zqmB-5D)E8#N$;6ldo2l9PLy8nuO_IemXd{0CO9Cvi$vUUrLH#d<6e-=TV)CsoE`(WMiDNZ*6oaczM;TFm*1z5PDoLW2zQ*l#CHQmbYK?Wt%rO^PGh0;uo54P@u~0ivne zRc!914T~r5fJ0Af$c4$BEWSQXcvPXtjiaxD&mSH3r{xDGiCf|5;2g{`8qFcWP=H!mC;ru_+fm zFIWTZx1lTiGLGtsq;|!^u|Q)B;%6S z`8r^?q9K=GsxR#o7>d`cCbEOB2Wh$XUE)`Bnq_|rhUTdA_|9RdY=0t>>EBXiT=Nm| z-JMMD<^mlOF^mXOeW>o#hdU?l5$>)@J&rVz9WnJNxjK%6K@KV(AIS|KpC(ohUa%i# z2HZ*JzWd_Un;YBp=0?umkgt}9_ZPO{jY11LRhYw4f>)3u6|G=1urD^PiD$a!meII6 zWj5|pCd5{Z#_!_^ov=fX7b-WSTW|%8xig-;oomBh{S3yoS0Vh3WI184ACTJ1i}7Ap zU!E>=&{>DM^9s-3paom;e)B1an{-ab7XN^8muew;*)@3k#)7(S0*L&54i?Ml(nW_I zI1L((`#VLZG-4OJ819F+;eHrDu?RN3S&Jrea(v0eqquab9n9AYg#I;a$QiQ%G|SJL zx6W#V<{3s1qgMmn(L1rHl{fe++VUq!spwvrhz1H$QqiLZ`j0q?ZUc|ti-9-j2cNfW zfSm(;5!-RS()vQMPF|w&edp4?(fUN_c#^0jm6Z@)@d8^zPAOan7>- zu(mp#`Npci@0y+BCxeSv?Ntl|zYc+yc2$s)yA94cs&FUI96F^=ozBnmr>gcR@YJ_r zcBwQ2&kU=C^_^*!sX^Ut}@TF=^R-`C#zx~?6ikn6rs=6@gG9P^$}iteg)D_3u@ zS`r$z%xdMjknkY?urU7(IVoy>Vat|;u38%&wr)vyPIgX;;{P7ZNl{w0^#6`z=Xl6$ z&vBMroRg3Ri7AHE%$jZnxcn=WfXe+>aEnga-Fhb(nw@kFjF%?+-05X}k8$NGY2`i`igLh;q zR45Mz^?Ba}-<6TndOlH5v~dxH$JMNM&kFY9<6JUu)D()XwCO_cS!Cldm;HRb629I@ zfiV-Ch1=ULXpWkbkQnYr6+R5+?cFDNte4BYHtNvm))|y#Z@|WPl#;^1u4}CcI}0Z)+;N7Nj3pOSX#L-L;H$kYW^XSv40)Q>4`+3LZH(B z1T;|j|A*6KN_X;Kqo!M+}8ThNMR=~o?l3sn*%A~(j;gcPz<*3 z#*4jTJ+ao^44=u~lQg8C5vuyVhv_-*LFPa(=I+(x&AmDyz+e{d`}cvcYq}C2f5@5d ztEt3!3%vN?fk*IK^(UUbJ`dHMCA?s%1J!GIZu^`x|Pyo zJS-^N7*p7eNNP5VA@xwsN4{B(XM5f~deR|kKJgK>${4N7P44h&s1&5XzQd^l1w>nH=oeHNl z32#JeYbQdkaTVXvxElk9`NL%6XqGoVi+sl_OYi2d0j_is&A*>z{oHl=fHZAUz@Ijp z={N@p!%exZgPc@$o&r}s7>-@`MmViWmrvBRkb3&*qDAje*m=f9P`4Y+y$9(Ko9#qz zRc6rxl_g~3Dkfo}66qJ~!~2{*^vgOM#&#MBYo!sKEs2+`So{|JhD|12_f53;%|&uM zvYWQAiHF}CZV`(rVC8+HXmyh&{r!DMC^ncRv1vLX>F1Ru#E6Pvnf_e9y6q+Q+K`R= zKYxY)j;+PlDzkBw>rC;f)<-*3eC>|tBY%4#Je)e`e#i4>CfZsk2~qyFb0QR z)p%b0F}w}mi0%o{8lgigq`D{^nZz3(Ch>QBs&VLZduabzOwZ0|LesI|G}+OO&K)R$ zM>pbNb;JbraH<7ABrg)?%=jhI?;wf8sDC1l+0pSW=;3Zr89^&}UT z<}s43(qDyIwRT)6(-2?&JDUgj^hf8vp5Rj2N!~*?k)4_!Rr>Vf{cLxm=CnxseR}}5 z8@;DBFASLH@6Sx~M;nJsy8$Z_&hcIfRieuV-)Y9jM?B?1Z`x2W3A5h5z_nj=`Mkqx z=#=jSc6`MIO05m0u%8dv*j;hlD9MT!vkb6kzR&DmIZ=D$a4;Ka&Kxr9SaioU*0HIP z-S;x%?+uJWa{3)r1fGK9U7m1TA&bx7^9@WUSJ3;oDd0K#46M4`B{3Vf1ScFEM9a?^ z^A%@?;>Ve5`PBOp7yGfoSM8BCS2VFFDE_acmCA!iix*b>*lktvAvx> z-E4xX@3#tRzshLy-vRvI+h*`y;mo1*AeEOb;-14yg^9CwQ9DQAdk4j+D0ab=* zwV;Ie|B=W`Cyl}mQzdD#qZQBe*A%xr{z&D!RJgxlFx`A$LGu%AVZY@soPRL}NA8dl z0`JX-)IklDb08UGAEiK9-$L*{lL%{{Zh^qVWwgP%3Lq+u>pjq;88$H(JK2PGbhMJu zx}(C?_d8+Fw|IU#YdCH=oC^k9irE6ce0pL19jtz4k^+A$8K*r&=u}LBuzTZRU+P9~ zma-A#uI&Ql6~p+0mK2z=E{(6>Jc17q?(r{Edr4oN>!z5W0G&fFV3*?{wBFhP!B%2Y zzP*<`XJ}AxSp{@k594|xeV}S)p>WrC4P^UY7N%yVvF$~gU|KVc-_4!QW$#rAG8>{D zW$W!>Y4}pe4spV>t?}4asR{-W8T^Pa3P!Kc#O~XpQLuEu5!d?hO@$X&ZskxKS$`86 zWFFVtGAg9&p4pK3eJdVrRKcgd-e3@$3qv*M@usz^c+_2)mb`K!)qFeZ9Pmx}Gx#Cg zh^rT^5bNTtXAdFd_+L6U+HM2BJs0A#2A z7IymI66KUmf^RoFSas%2`fZd*D`K_yoKc%4PQ#zDZzJ}x={J4ALp2bbz73<1IahFX z`Z4-7$s1Zu4&hsrX7N+w$M74{hkTsJW$B5+7%G18Q@A?rCU^f*1gnNAawjE(uZqtk zk8fq5@`*N3I%SM!rt0u3ANu1XYel{)yO~YT4WfSqKBzmqR1i|D&_W@HFPxf*Ep8Hs zZuFzRGJ$-!LkUV3X2Q#-SJ3Q(6n0r!V*8_Gym#O^ym9O;fqS!Pq}?KHmK!D1W}EV) z%X!Q!S_(5eUh;99Bk@1|J}gP^xzO?U6sWYF6mHiig0^M|xOOMQ&7lB+=PmJ}>1Im4 zb`#3$T4;3lPCmNq6?sQQ_tcB|T)FO;rCVs0avYE zg(Dk^C08empyuGig2MQGHt($*d{Dbs)6#Cr6!&=%8qS~tlX8SNcLV55c)a*pbT)5~ z@4>(wV%qC>m`@6yEe)UBBz0S0$6G)3=9ep9v+AG8bg|KnUs!zz(px9v*=-g4QMwl{ z^IpJz8+q_?-(m&7RDJC5RpYt~E<)k`CnDjWAv;k&lnX~CY}m*su83Rsx%F;XsW2SB zd75G2${<|3Y#UkIwbGnX(ZY^hi7+V)F~9j16!uZTH>L||UGE?g$0bQ1sy{6Idk{u1 zwiI6U(&O9z+eTV1kFdm@F}yr%qeT9B8l-3K0?*hoFxa&W9*iExb3WQY$s93mPrXb3 zVoh=Df48CZ_AjVBWXdPjf~a`%c3ShdoEqEQK)!A=cUw3fJcIYK(AnPn;)A_H-|A{% z*?a~s4^D*$7{K-!KLzDhQ$Z&)1$AtS(V@ssyxVIPH&{#vE_czqx|F=-hM;LfkYMbWII8MAIcaXKy}#&zNYJ(Wc2)-T)^8DqfX&T!V+Xosx^k+5-1 zouigtitxChoLVk?7gf?>SXO8YZhg*kL%yGDm|c}vJIQdfF`sB=uXn89fnM-RJ^(SUWW)*m=Qws;o6nQC%L^|3 z{;Rn3Wb-jR4YZs4OA4~6CQ~Ap&dQ$dlIX>x1!mNWMG0AT*-un~`0eZ*a zS4VIDe5oe-j*`a$>jZl9GM&P+_K@Y$O>pZ@G5P;ebNK#39q#)i(Y3vqq;>KxJ@Is7 zPX^5sX^ahl_JEi8OKv^S9-zKpcHU>&seOX9&Qd04*cB>SXi1%7VxU{Cj3k=Mdl z$A*3f`As)hy8l8dxaRM}sAaiWYa`|nB}G&>|0*WB8L|Cp4N%uKf+n2XEDDJpfl7%l zNwXu7mLG`47tvMt{Z%gS$~l6Dm;2&~qB%8Zm(Ie7{q^`RxEUT7kH94Z_wuLVMtp&P zA{Eb025Db?uA@-E?XPMJcAoM4Y3Ncu{GmBcsK1ZL)b`MkPd4Q3+#fAJCF81x`FvcM z0h-7T06(>>Z0y>@Y>AQq4|!{j+Xk-Zx!xU+@XC_Y>nuF&FoI`{KS9bbw$k@vTXH@U zOVQr;XsvBR-i|AAnPCO{8`yyEF;RHOb{-$QS`Et|o&tlKy|laENc4TAOe0==MZdy6 z=zQ7{)n~bba@{x>QqUhXH+8XzcCuihKMfZYkAiJ$FH-e2J)!YXF@?_?DGrj>QE>bk z{H5}d#=qYN-S_TG;T zJFAH0ogcu#s!X@{z)_FR9uZP>qN=AiS!U$di-`zwrJXbQ#r&u3tRrWqF4%iynR zTFiZ6JAL%;%L6@=dE&b<_WFJyaCN9L|2wERmKH9O=IMl)p3}3G>%`=+)i;0l@PlumyIdk2S;wbr_?>)>0IV(p?Be3 zVbSv4v@O|%<^-q;E`9e2WepZ=llVD%7hFgh4t+5+-h_X(ltS4r1F_8*f4cmsiGLeD zjqg~mNIMHtB!|zr(%wEf5HY3?{=F%Oicf26rW%_`B~RLD?j2{b>&aFo$~lH^5nm{v zdoI6|;{(W=Xv^m-aJB9gpXlEOEjj|UL=F_sdEmnhmj8pnC5afxr}E$1ydbtS0jt)m z2eGv}|8+E%jvA%nX1`X9_&b$14H(27d@j;1g9)N!RU2H|oxu(b`$?)jx!V6qH2rOo z<7)Otac+=3_il1$K0BpM{`4}YOMe^yvIN?n+?wvAmM_>-k=E`~QV zEcthZofuqj3D5kLqr_!T;GmtZ_>`Z%cvs9pemFS}ZXHes>xCbo(I6JGuO&0HKpmma zYF(~h`Uk7FstKlg8LaKse!-#UCPY7;!js({@wk3CeOz?2rc=L|J(#Y9T4HyY|HXkT z>5ZqAf1dK-!xzwQ?ok@2)x=}h=rC7VXMAFuCsDpJiyQV+L;sr*a80C7E0>v~PSYm3 z`O1(+7Dhu*=prHF`EI`bi!-Tj3t(!7S=7JjJ1ltVipFjOSZ)89p1C89X$>wEU8qms z-*v;GqkcMqjx0WVGn0ZgnII4N!TLo=gbxY!y#KY^basCvr9WMX4>l|E`Dbi#^;k7Z zXwk=yw#G16b|}1W{mTYldBH|GCgb48d$9ZNOg`)95?%{qq%Xz*`&7~g+qT6?8_SM! zdq+8#>YUDF+g{UN#X~~Dtvp({x>3+Ll8M8<<*`@6n;<&Am{)Fe;cDOZOYYSc@W)@x z>CdX~a8%iiSH9*vSaA=(7PFi$wn*m#E`Pv_;y_KFbkhOO@uwM-?{&Z zz92q%7~AjK^26?0;?&~Fl(POgc-;4dJ)T2peVPe$>^{quI#=OOx4zQ7*Ye<3*gH^< z^`b#<7jWC4I8vGlG`7u!%KjmpFdd2`7WKuy)9=zyOKtw!VH_^I<$~vLZpZe#DB7Z0 z4oS|X-1DbDpr;$A7YHOfFcL%NY4L~iw$SP0d91*rlTEd{Ap{m_u!=Ji(dCGbkT&N% zHQg}h@85l)R`EtY^>-QG7I%PCud}@L&N?PzeOUA}M+qLfDDvg9i@5xSZ$f9SCg0_< z9~J7Y_<|vMpf@63c&8o1x>vt}2__KuPLbV>HRe*Jl_+mGGn+#&$~>Eyn+t8W_;Zd0v%KL_}?#>$$&)$Ayvg03FsXwI_y2B#s5e9gt{$GUffpPi5Qu zM&e4pK0G&RA~qO}fX8@{*=t3To8dKK%J?c!RCz`3cWH2~N!GyBt=WH@)M3lH_jKOa zhXQ-}r0%R}X6Ppio4md82?gNA!;879VF$8Cbhf5tef(D>(WXdSKk+3AMd1RmwNLtv$SDd(LM-2e~j(iH(1cK zbEW|>lU>$+CoJe&%r|dn5eBU?gpo!9nkY`i^zwDM;_(wUr{6hxcD4Yr-rGWQ%mHpF zE@9g9I_c}&8h$BsCBA?43wo_76UMkGP#bb8J?J68sosgD}<-$NV>!#|Ig) zpw^T8Kv^f&Ya~*+x`DLep_mU=i(m)bzmZ&wHxBK?#6wPm2>V)-sOy&wUz>4{uKd;K zd7kft0kIN6Z2y*7WIU$E`>{gq`f`bHs{=ghN~8tjeMHO8?t;?5|43=TY4|PAh4*(o zAxI$w*V$yjwjnyW|MU@|AX6C?icgaGdo#riSCGb)=Hihz@}zrjJIq|FQj@x8692yD zC@b+9#8lFMqlc3W>4jVo-abjdxW&6!bH*4-zh^--w?znXkD-eNec(}9Atkllf}bN} z;GhTctm%Vb%`I&n;ASi2FIo*U>rR8eWTDV@DY2&}rIMR&k)v8sy72t81t@JBM2Els z75a8<5)D^f&Q8B8Lr~MI9YZD^L34RI@z{p#cwwg}-HX^oHL8g;v!~Bp>G+9q z*f!c>{G8u5*oDq@Px+U}9W|r=%fT67i=qm5emm(HAN$l)jPhwr^}Iv!`HRPu274%$9n6Ak7(f{D{s(hk#^7~Z-E<}nq@&{w2t>w0)RxRfqBx`-}4 zILRLFu@IWuxv1#(Xih6Oa&@&b)RO4q{p2{*X^bQtQ=)SF^cwG45f)E+O6%hL;Afr1 zz!dBx+M5Q$pe6h9@(De<@kIs_hrDG&DkY*W-;E5GwD6Mi#&o%OBgpG$(zaEhe5Te? z%#6ECvO$_yT0UA_?3gTkY%gZUHKV!mEDi8o;Up?O8VH{QKhfjX7`Q6;78627QbJF> zwp#Tv1FJpRBU+41537Ky=;r ziRh*CIquSTJP%UZkLSP0i`U+6rBbOTS>~(YiaQPX)b1J7Y!XqW-!m|8xlXstGaQCB zxnS3XI#_e_BNu#ac}$%FW-vKvYs5J|KI|ENik}86HXH)gN254wsi8Hx+UROg2~jEo zaNpK=ej@t5=yGu*UR85J$I#xWvi%ng5Uu9@V-lHCXBMrwS<7BMIlx68kA&Y@1L%pj z0xr;*OXq(k2?zU0Ab$QV3OPHYhZ~82tbo-p@77pSTvEWZ%d&7s-coq*JQ0$%?SScl z(eUPKHO`%P3P-k=^44B2VazdgT-Jkm_7}tX^SRZa^D`E(TppdY)99~B0{^W73I}Z7iOTkI1J6%h+_l~2ibsk{3elmEn0y^j2L-VM<$JAB@uBjA& z=bi532GZf@7DqvF#!!sVJdXQ2%z34uAvNi}p|eNjV3Efi%+!(c$7wUEZ>Y8K@x^v} zYS&L{p*@;Uzhulavhw)EeV?fJ*uLVyc3SYm(ganU_4tb82f+QmQT)T73b-H82NNbg zh5J=I(Z2dLR9l#%scIT`e0>{i8m_^tDaV9KDThfsx)l1Td9$MBH$-jCb?{Wh1FptQ z!QBS;X=VIudi&~`(CcA;ig=SNcooaiS<5KiX+4iGJ#>{PT$I7fxt;KBV-Q&gG4Lc( z%nvmObA4++`Z)R;CJv~Um@y-Ma#gONHslrz-v5v8KDo>a_CFIvXg^|^CMIx0EF-3a z*P#FC-}Exj74$8lK(lTu-)-e4HoE+iZVTIa@!fKm9GyY6*UM>v`f1p=;~70OeIy(y zZwC9jc`#;qF0a1&h8K+;#{<4dsq}6!q{%&lAE?x$JGAgqTLeC&pFP+Hj1y~`wV+3i z32%6}jmjIUs9x?A-itBDPlYL9WHFZR2ijnAY6HvN{Ei;~PQWkg#-eHH!=Z`N!z>ZB2Dy_#M?>=%5Gb;*Fu_50QCt7f7BgJM{LS@@Y zuD!gFCAydNKxvosUXO z*TK(o?oi{$p=3=Ul*JUXC1rDkMn41mvAl(4qIn99Oy<}7HG#TI1&vsq4*o&)R8mxcmuio5 z+rmH0edTXXqr14pAtp8|Z-swjZ_$*C)=*l#j!#bVq1nCA=w4%&?#+Qsyzv$@Oq#yy(L*<7;H?Z zS7qbVMK9p$sJA$^u7Ed{%$Ayc{VXirC&z!vJA-Ln5bs=?0v{Ir67r6Zz_yqyhq!r5 z`9qKO{NB^0Xg5iXC%hfWZw24t7le!8Qa*rLJx$_14k}!2k2-I;-^I;sj>E#qF@ndD z?R=@;3%rq24lQ?n!0o>NApP6M1wS8;w1kD}+B7gvq)y_|;z5*pJJr_?D-eIqv=ewn6%we|3}2d%2{h z`%Av0eCTfIaybZ1|7s!gxj$Kz$kT_$qcnZjNbZ^S6OVV~@&Cag)$x1qpI5U`cTJsE zF1ii(WuNf!Ms03?%@!ujKZ0LVuEUwhH+Z_tYLC-`>z!kme+;Cx`IaQcrm**@ys zqYQ6AtE+K*^OChZ*T_t)nWl<52S@V6=^>ENH-#(bT2k`v0i0<%;acTncz?DRZ#y~x zN7xwhY4^M7`hG3(_o<<+yQ7>hwWT*3LF2C>q zHEAE>3;PY^gNxJQsO)8&vRTaEcJ}o7k+P8Q5CsK=8=yimk9NOb?x-HB3xl%q`38d= z<}TX7YF{pvzVz%R4ZOUb_wzna(@vZx$!|4r_5?RD;S2bUiHn6>#ko*^@DtZ@d%#oc z&+r!OIx^JM=UZHT*!^!ih~$qjI2T0+$A1E!{R=>U{Q{hngFB^TCRzX!IP?}FsN zO5S|j3jfsi<-ZKeLFvCsbRZ%GRrS;Ps7n*@!Ez<>i=tQLQFV;`;x`LN$Dg5E<8+w! zGY#j}@8ZFWX4B_WLnt?=0ZZP*a5dFLwsF3eu=s^eO?~Z5<{Tcu-%QV-Tcb|!{IlC} zorx`a8BXC-Mf!Lz)R@=lG|-dYS(q_&8^#7MPXX{*H&?x({XTa zRV_^kIfRe4WP*>q9UT6hPPwo2s5Th+U)OqGcWo2zTs;c)dT{hbgbTlYK#Pw!d7oQM z8N`?FlM`o+odg}XE>WgzE_h__pzdjTr2n@B)h?We@87SBevCG!y+`L_T}~7$I&H@` z=v0X=Y*;Ufs8_<=>~Zu*9?`c)6WR0Iv&S2=1H!lbqe{;*$>c&0=4L(t-!9lLNPZrt z%l_lYXt%#$zNd+7T6dtzfXguA(N(b4GQd6ovN&(OjI?fie>^%si~EaeaOHt@{Eng% zFSj<4*34ch4M_Wsu6~nHZ&U2i@wel=OWknt!XnywSD7wu?0{9rH__xWCF!0Ih5W)T zUy3>X%R#Zi7#l8VfMNG0>Kx~RYn(H2=!L6}`qN_hlca|{@<$u&2)id3&KkfL{K&xp zqua1o@LaCx;0FJqr(yY|k3Dra4&t2pz)`&xVdr5x9v#&W4-fDMYJEs;0WshZohw`| zOc8W#Rtg@^GAMq94;dXi&Bq2S(*EEwp|D()v`&p6{Xi`&-Kc{yr>F7Hn|m~kOVqbagykc z*G|XZ-$x>ANFpthAM9ze4YjUbjsJ!<3-f10 zFFia3AIv#L@%J)#cF!zxAj(`?Heos4bJN3xf&~s;g!FKtvUKkL<6I{88v}C--1OfL zI{Ksz)8bb#TyT;SX1<}SBZ$r0Iapb6zQS=n-;uhHJ$4F#(s&EDYG-f0XxB)5m*CAi z@`h1xr#u9#^MT(!eaIn5n>|uDM;-U++#y63wmcZaAoc`abKl9Eo2JvH@mpwEUoU#N zWHfC{$)ExKEchtq2VbhcGQ9*RP}k$ATbstU514|6=1$4O)QfCUa2QN|y_I)w$-%VC zvGmj6IJETS&}R3!BF~y4)-8E1+|8dR_z!$d)uHxq+3+K^himcsx=FnBO3z*R@t+Wy z+M^A-&K1wv^&eATzE;q(KaSes6F6hb3gKGIem2;oi{1TjfmgGskocjNFIrIu%kF$6 z=LyT0e_kp~d6Yx3joPf^ULLG!_zJkDnVVfYPZKYeNR*0I;GS7LR>6Jt(LjaXN^6Eyk8*7J@Xa$oVdopFI5U@PWv9W- z>NIK=RnxkwGqJvOAd6qJkHt)k5w;~bu>wg1lO3AJB3+kp|9981HDxDH(^thUFr1&9 zIvqTt*7m@We~r7`8h(DjUf7oZ99vEZ!%K++HEfn{bE5zWzwFQY-j? zj|YY93E$bykp>*#a4)IG~?r(deJ~x1-@?SQ5yv3U$E-HyWHmAiP#r5C*5Yay<^sVXk2cH!0WP0*P) zS(H7tp4PXb;Kff0@Lpq{_q32=gKn|uSz+8sCXtWuZ6*5_chKwPqP`AG zL@U2)!k?lL*0tiKW6IH;P}Wciu`{pm@Ke9ohd!HyGeyZ5;jjeSf}>66Y3YhRFz!eJ+w0Qf zNs-f|$~kvo&V?RbCR-T>9iNWM%9>~$SO~TuU#Tfajq5xsz@}#rsNJqZ_ndX8{lW`y z@TvosOJIpkKNm^!chF9o%3I8{}TVrbuII zo31bYIcO0cTb?3J^IRk7ILQl6ZmWd>3yR@WRT(UOa6q!wFP?9<>JO{$%AoV6S^Ub* zDS+J(!nB2@f>Ot4_*<$bUX{0oCJi4+MK|WbjJ|y_xwc&L!{G)iId4w7^{Z%`zA0b7 zVFsR9znoW8?xU2j<6PodhS@p}nw`)GiuGls7f8yttJ#3Xh$hKG zY`}&$UHs35d$98IG}O^a;;sJ;#~l+N@s>n2vHnFRv~By%s#H~l{vLzG)6P#2m-Nh) zmRI-kGma?eo?eCv!XsgV;Y_i)%rl6ZY=;J8tl;D6T=3ech~s)crdQJabRb0@y#iwB zK=mFdxuk=YXUup=k}_A-i)34!UkaDs7(hQ&U4FIq40=CCK?wGmL{T;N!oH2R!u$(U zd0!Q6oNd`gduA8H>tToBs4#@@Td2ToA`iovBl6@L_>mS*&f)f^iJ;qUhPI+iKKYFv zb+6H)Rg#TNJ8=^WR8^-}BBa`dHdt#i0}c&NBK1C#plELu{WgiFLBZ>=H0ud=rm9GD zFYbnpd~K?+OPj>^pk&DsZu6)Ro3DsK?Pdrj%rF;jw~rIv=9R$2 z85>}!rhbj`olu;sJbo*%e^vjg#0Gf@AGwb z?0z(_i0MM;tt|Gf(U8jYe#?sI{DUy-SZG zN8Dkmblpoo8+>R-_%>QOHInu{^TrYDE|L5Aop@mK4gtNTv@6d7Jz6gaUZX#P=(~q7m=)R5S>i6q0^v+fGqNfhupS7Id>EZl0 zKYIu5)6SAolN&d)&H`{~hjVQqJm^#z1{})5&)zjXIhe!8+P%g#cl_{P;3CTGI!wV0 z_7bx3!?g<8&VzAmY3$6NoN#9M-c)eE$Rch$ zrixcM72GVdbZ3^I}mZ|utm#%oY^))OE))r5^9ErP=^V#W> zayagdGF}+wgYC09ZCOwx9D6n!O`?=|a(x$i3JTK5)8`>u)XKiSQWrJe(ya+Fe=Z28 z29UI0D7<>>M`<@+!Quw+2E6WUhP>3`J%?V z9&6#Ev>Uj1V-~5`=OG)u9tO#4@P&_t(4n5m^xX%2zM!#=7g!yo;);8Y%L*2tL!ttH zUzEsFjov%1NZnD><#bMDe=`{qruy++Nj*M$^PYM?{~ny#n+%d}75;MbP~I=BjLdHJ z=MIKS{7_*DQ@9Yvy{0_iE$460kO}+X-b)w3I(x6sY3D;P=N@Lemo0}yF`Fp<;akX( z_rufbnM|p0Gbtpgi_>Q6OTA?cr9(a^L2cI@e!|rRAL{|0(`XUo5~GA_r6n|HZ7|jU z=!1(buEXBV1IhU40{-E$t@v{1RfsteK@XCjiN22N4?3p5=-BgHxPDL}$!mn*+9~Q7 z$@KYFqXu>@G?NdzzX@|=C*WQ0V&>kzm9F)91B$mV!UW@XY8j%B9|m@_vb%o;%@fmL z&Od7&Zar7H8mP(p?^y#GdsQ%)mGRsMS{UvvLnD9g5f0f4LP|hiI!k9D*LMI`m;YsQ zgB1AMNy(%%QxN z+>G$d-lzB_p&ep>{Gis;1M%7Oap)61O#C>iKW-`E_)RWN?A+r=9h#>vz57m!iw5-c zce;tF=An#jsp|=2H}EUXdmu{G0kxr7G@hrK7@=*e?wUda#9*Uz`{cF1ZL6FQy8jdEb$ zc1CfRM*=0L#z~eAn9RK19EI171$1-PY3Q>t8(s>7A(8Qlxhur;N+piz9t7ycj#qYGU_~T)>?;QpHID>ZAnrP;EF^GB7|+DET1bVJWe~4ub-SNIAy`U6<#O7%SOnvyk;cJ+`CB zfqgQS!F)R}W*DWy8&p5hz$<~U`Fbs@_FoKBVp=e8$sHEq=*C;ToblGe!%QY+6HcBr z7JI2TQgPi~>S{P6vb}yzbm?d`<*kgNjx!oq8@Hd|KADHN=iOmP#>(Qm-d%igFI90^ zk+L`=_cMRczLHt%{e+Dng&_SI01Gzu2bK8oq#2hY2~$$WSqJGX;%XF01l zxP<9PXEx0pA5V|gP9pLvp{&=zmooWJ`!b6tT21}9080~gl6xHEZ*Bv5)j|B(=luV$gay5 zu0{?O_KvrZ%3JTkw|)_D(nF7%O#BK*eyK|Re9SKnJu;1qPZ!aIL#7lOeY%G)mxHq2 z(PS2rA(Fo)u<*K&p1U3<{JWhk9RK-@1wOD9KKEWETG{+o@^0E@CO>dK+`qpT9o{_` z6apIrzqp>=w#zC^8fho7aFxZNnh7xAQ6KKMUlz*N?}h6PcfoVPDkwj7jzR}-;(H4Q z@@G>L*f-OY@ZkD0Xc+Pco(&qwUwpeue5DiypU!1-OkN6aN1tW#<@OXjX*Ny07)rWV zooJKOenFzXkU{QIVf*V{lr<)i;l{zF*VQ5UHY`(;v{w_3EIz=(z1?Wb?^CpC=3~d| z_ISbIlB#1x+i`X$=mO0zIYuFZ60KkGS(t4!mEYdu4{C4Zd0NMDj0hhH4{l$gf|-5c z*0L6Hw(A{f*H?cu_t+i5{o84T&Oj0`YeBcD_4uxM0%$zg2Mc4bvu}w# z`+8xP?3JDlx9>ZS9r~Dx^Uv+3X}i_2sxFB)_uq=gcW3fvKhBB-Z6o|JZZ%qE-N5AG z>e!}|#it~!;@ZwbVaxdb^whV%c%8Hde=fH3M1{Z1cx*A$#F|MObtb^Xhebj(lC{vKrEhnXqO~NDUPnl#wU;Jm$)!-Wbb=9Yu{6P-F-+*F_*B(jr9Ve*N zRu8RuW|$}Me^aZ@d=@Z#C0_0E03C{LqeTnKK<;QfiXK_P+*^%8-_16n*Q0)ef4D3f zNq@5K8kXG1sTo3De)1RMY<{lxEd@@GM$e;VxaIkE9`Wx8JXzBQx7jvqnUo-zvoVdd zmsivBQ!#wp1x2av>|b0dwUDc}7E-~wDQx}FFz8Xba6;U6(O$P>pj!8jwE7RnF>AKt zq)*4GJWwk5W@eyee@`-ceF|n7ZWO(K`;L-VT;b^(?=kmZ55d`}2r@=m;rh(8d|cdf z(Ul*H*xDTh*{b;zwl9TNN1r9k)H$@+4 z{5nS-UYAa!Zh)^cil8;Af|AZCU`<#idi>G^gZata!q^>OmYt*`+f+XC{&18`Ge_mJ zc&>4+4vzRoGmR8wmYf&@XTqZ4p#2%p?XASqr(P6EgTkSg`7`u1?xf2H}%81`N zgi4RQD4?$Eb(nU%5|x9T&_D4An6{;}&PYe@I<1f|bAN$jJTLGos)(+`$KpS`e(3ZN zvGVy>IM>N|l~puW4D5peS(aFUgCW}EfS_3$h5JmGVbmc*rfusz1}u?c=Gb>i$o1t^-EKxsz+O)r_lTkiJ4 zjq3W?c>Ff~2wI8LavY$2j0;}u7l8X7bb?){1q&HiBI=bh8dQRoz=_Shq%G~2p((qW z`tLZ1r%HOGexM)kr(A<$4IRX(Yn^D3!WbwYZVD%4H-b)xB`t9NOV?KIfd?z1sp@hm z_xcpd?XSD=W%mDQ#+RG4?)5#UKRJ$N%^OW0zwDx+ljA@>WHh%fdP#wIIqrCuiHZ_y z$(ZgUNmaT7&7SCnU#~x)7m0#oR7(ZFKBJEy%}DFPv+?*axDk`D{-pRccj{aCo@F+l zplj#SgtBMV*mTT}yG}SmpEBgJ?Z;M#Ip3R3XzT}-x!GWIVm?$J*at(#PlM_gvAnC! zjtBMWnPUt7NDf&gz>}VH3Crv;No6qon7ECO8rKW9Yt4GNqhJ>NT?f)8cEb#VyAV@j z%WR8Rvr(t6u!P=$%+mW3*5@A}gD4|$$hFVBXA7BI|I@FTb1)xe27B_e;UCbg<`P}k zF=VsW$#8vfhD0MMU$_zbNTRUC26O+Z;*1NiG*aU#-`1mdXS{IZQFnE)?(cM*lk*3> zj9W1N$A0dR5{=y-jX3Jmpv+%+zNIt=_u4!V_IO;z^B#m-K7YXEZ|8+B?-&dmwUY-3 zK{&vK=@ z_j>h0YE(Bfsu)blPx|rtE%&Kvgp`&qvt!5moM&Fcm1u-hvtYSu8}79*7R-X2;o}?) zZu7Z->sNmR-=f0^1H1*%*HloAZ2;9K2dap&#ognLdGX0?sNS3oje_iA z2g7j2$=x7r7>A!ldgL`NoUDUw;Kz)q*r0k6`!$!~!=d&#KPV5^Twa2yFXBlpvV&4; zf@}2DG{nwbrRd;x7%w$O);Mb(pv)(K1m_uc{Lz`^yz6xlT-E!*Y`4YGqWwOw|Kcl_ zv3(!t8~I{uz81`}dMU&W9?vb>PD8iOR`}^5f}ytuLxbKoCU!1ig>{u|OCNpw<`cqK zOpfP+M%v@(b85V$^D`gL&p_qvyO2^EMtf~7!O^{v4%XeG847dJdfXHY4N9R$1LLVS z=LiKEoh6wa`}vzu=N(z4IoNuC!o%9V`Hxl&Ox)H}%Z4}ci646LG2W-?&8$p1x?wn; zd!dUrM&1yWDfE(znv#bWV^zhXaZ|DKmofjd^fUU%U!mLY|6dzt{+8nw_WeduDJhkr zfhZLj3hBPqxkAX4u|j)SGH)uPD3VZ0Bb8E!q(O*^)P1dURVdq#XkZIP5)sKfGWT zBjIO2c@nA)bm!LxQO`rAkx*PF_{lZ|NI!gw@z_5_nx+B&Mt zJ3)))pBEObOXK%mRnw#?LFgmB#iUPk`9`hPIKcKKxlxx7I_cWjo^XPDT{_54Hdb@4 z-9j=ZO(Xi-#`2Ept4z~H7asSX5p#ME&>6{NvEPMJ^pDaZ`equ3?j^r)@Ye`_R62&r zMTEoR*}vg>*F9mBYbdq)5y$WNxYB&KkH3}+=Sn6osM!WrrnRmUb!7Rnd9ziR>4>IBW+;qWRnkk3&9p0kC~S1l?y_hKmh zuF@axOANUjX(CCi9EVS|M&}?G-d<{luO*5+$_HovvFaArzS%E3|d*d$HVL6_*2QKARy_uVt;BTO#c-I%69J|>7+i#XEk8sqRushZxG(Er+mTEy~IMj zn`V!iRPiz-8Kytn0UI}m^YrV9_*~0gc(daxlk_*{z8gos#2rEhlgmgR{s*-<`K(vwOa64s}g^w zcY_Z~v?FyZjx&=+U$Bq1;U}9ogG~Jz)DWnt_3&%c7g_`?)S+nmUzDO;F-QKHAzZ>nQBf6V0B`%pyTs@x~ zywIo7{*II-52ogs@49Yez3p!`t`{h`U|>BYhPfO8zoTQ`Na9g(rp=_c4X-h=1;yn_3? zLO^$aU(iWD#7yRw@`$%jk$%2_ne+EyT&o>*SYrVCW13lTY!$QXKZljhxg`D^rU%{0 zfpktu4AnK$WHC z;WHiQp|te`8sBnd#ZS(V(HG;n@8bf|WNjPk3>oaWa)}bpnxWxnkfQ3Sw#0>Y&yfey zTq!iPo51+f$8psZS*`i9M7ZOU$1Hcp;|AyBG6q|u6&9zoT`kK=I(_`S@ zC9jxs*ergzQjx2fjluqX4KdHsotAq?(cy3%!-^Vs(xd};PG>$gtXHm>ThoP8>#g_$ zwORbhXxYyF{dpQzT`JA%{}%o4O#Pizj2v!hLMon?7KFY&vWnuph5^NpZJ{0_IdY##dds-!E<^e_XW+7)s(!TFc$r`1F{B3 z;KW0H#E$RTbdb!K@_UIH{8d#9DO%dpbM|4fSKSULKk#C6<&9X-&kknu#u^VeW#Ea9 z3*7i&88Ejg;5FGCb~M#NQut4%S)EA6Pk$_i7=OTq_mTM1U@BQEuR>0I9wE_B$d=qx zu7nv?AHh_w4n}xCf^)(U9Fw<%7tEbVbBeB0J>ND|o!yN$&JO0NBN25BhE(WERN;o% zUOdzgi_fKYnC%_{(I+J2>ak07-ajMgsHb;O^-2fHHH{*sN}9Yb*_-}eK7?APO{9{V zWpuITMVuyc5WQ{t#hZsut5E$JP7?yx@n;H1P36x(w??$%FO_3B+o~T}{8fpT(mga@ z@iyOb&78OA-KA9)=eYUBhgcEx9h6Uu6*|AX0tKVBbW*6l?5=MFcYf&6zg(-h^_XVx z**}4u-D)ItN*yQI=ZHd7b2c2XmMNibhcov-5p3~}a`4fb4Vl`9sOy%gxc62hD0n>u zHIJ(h?cTvQzkP+}6+Kv)|AnsER>ifu_3+xnRQ51+0EV9&h+*NK@Y%POjTnB9`1Rzm zejm+Hqop2v6ul_D+oxiYWT<0XlQ*6Ts79}zQ@llZ2HR&^V4?3Q; zp}~pxD|RVYpMRf}pS*^9H*}#@Kp|PzGYsQ{d};mUf4TesZAZzGM_lvFLAW{88a3Y; zIOYxOP)(3-9=uYJd%fo#PiE`vbVz_oeqnt!>Cq6SZwl!-a!gJ`D)0?eAs(&W2N%QmzGVP701zuXSke^>S`FX*xZJJs4WI2G2J2;pg4+=#DxYGNaL& zt#~wpR?d~pZ}r;zb7C)#deI!uiJ(dY%fNNsNL+UMj~IP6iyoQL zLU!az$?Yr!uvb1xemEE5KQfledEo|JcK#Gsx@jeR9il^9w=IJ^`F=c3`-&LwZamC1 zF@{@DgXo*Pu|np{-`L-NB);~YM=>df_-)w;vzKlamJO;B)BEkGjt4J+^xbZB`&>tJ zwrz)`jCy!o`HOA8p;r;twZ-wpZ#_0kV+y~qS%k3*#{VBhSd7wEZ{KxEt7YD{|JN2a zK&B4Uike1;omIe2Z*TazJe1u1lO@b)NyU5O3Nm}#B5X<+fkUef66=y=7?*GSeYDm z%wV?*7sB@qkHzPGc40raCt|cli5PYxpRIj(nl#z}k}TZum3#>gB8HEI^3A8$qv5G0 zvLUWNiy1Tl)&(h(QMHXEDeRum7IIX0cUGBQY|UV;2J4CX5JlKM;0dwLED@ZZ2e9%r zYe;arHH#e5%#>!QlbMm%iT$5-=;l61DAU=&2D8~Lx1fZ~nr{J>>W1vx<}fH9^O5bC zb4#4P7Eu0J0PDNx4C`ax$l5oI!7VQg$jCMnn*;WU6aS49^CQf_wmp+&Uo2qi#_~8| zs2i(VX;9WVaTakcZ4(veB$8!q2VugcrEKovEH+YqmLzX~HTbCY2NQ?4!p0@p#J5i_ z@za?sUg~y&Yq#~`nZyPh)eFgSl{mI#(_><|v7hvoVk$ZB=Z0}(uEVZCHBfR|iW3yl z*@`ocBrom?v0XPBlAH#L_ted0&Zq@=w{$hx9MmAT225b9@8=06vCB!?u#dU!vL@=o&#kTC>%GsEu7kJ1y(6RLYv=gEDl&jbSB&qmiq1>voCxl+gB( zAn-^OHnyJ??n%eP(rce2>2u$SzF)SIm==9uuWC2jSGx!YhFTKc(suSkZiKj`E`r$% zY7^ddOpvCn`7NyP)nv~}3+pX%C*L#}@hCBeO$Ofxe4R}$l`p`6;2TmyZ9|;$`8U~> ztU$0)okVKM{Mm( zZsHL3Prwm!&LW1%sZ1_2GPH-{@T)B1#RfXTIPP&=WJB_6eekvEQsfj6|+oc%61&^Q80$!@Z1Q4eOi>aaH_ zGDUgaJ<_;$)_8#8@2U5LE&e-z}j5#gWo0crK?iBvvD_D zuBb*1TZZ6{W(m1ezZXLE(lE(+Hzq90WUihXxO^ldmlsu+IUiZbS{vt(4_Y#>PDv8T z7qyf6;=_X4RCPKyJc|S^v}0)tx3dB7yl{xLo;04QA!{n6EOWab0p6qd`g4nFo2MkAPu+?F;qVvIgw%UV<kk!3etk&aDz{MR%Y+b=kg z`s^>TY@jK#JuQIJuuvFYRfzFDuhJVE?=V>K|!? zrq4Cj_e3Mp-q;`f7R5k&+d=aBR4$mW*aS_{W;jw;6aU(H2xBe7$f0AqU`dzEeP~() z`W`!RWgEl8>s7J0B@9?u5=(h2yD^IA#jU|w^s~!w$op^+ub8Ki^Iw7m@wg5jHDohX z2I-0sCqi+->!%R3ax5FGB}1OR9Yzl)DLUU-hAN+npzGu=_U(2W>$y_T3=VI@g2>Ix z*#sJmf5$>V9#i&(GThHfP@i}kKFI%MV@CWDqSUomo=>s#Wmyf8_WWXLaxr+x zs2k)vL&&+8@#ynxA1*rU2uTa~kh^;&;I&JS*|3GM!Zrrp2VN$XYfaGZqJU&@x-h^s z8;za2#S#}a{Jp|dTs5?i?Y{oM3fH+XxVC^;yf{rZ#VEo+!vo}EU=499{V6Sq8_H^L z<_IC>vUlpSt4N})K}&xYu4)Z~WtO_+^4xeyPMRG__c|c%|EbEUwk>_CRSm2C$I|>& zLHOrLGFsJpLf`vD7^rzr;y&XQsasV|G)~WA^@-u|;{1NJsIDV&^AF(l$Empa=5<1X zvf1;V5>Z9d8#9i*Bc5+G(f#iTII&?le30ewu0I$_D~0o z8m7V0#&F!~@5t`i{$%${W8veVRl=JYZ`j(&=~FKBUyGfsFUUyGk-X3_4Tnvi#?0-; zqJ_s#)(|)zKKC4#wz#yiIKM4eQh1FN1tg-WRy~Xvc2BZ>dJ${Vv>|uX^I0+NAdm9K z!nnm#NWIGn;@`au7M+u4WkDNRLP|Oe#vB>TSk2z*9)vch7}Bq3B6~B<66$`1V9mIb zEPqp^c>Z=Zc9b5&t4-OkKE(vb_(Tz}0UGybNyD0) z(f<8G%yx^02wx<+UCB%`$RAhOT*UfYGAEhYQ=+kUC7Tv{4t|AP1I0xLn1zlm{;WC) zkKSv;?FaSj-@%RWG(n9Ozf-2|Hpj>VxqsR7>$&9RgaonkLIS4$DJIG@4+#%!>V(+w zF0kEQo4;(SC21bEDBrY89MwHkbdvwb`j1J)o{87cf;O^Io*6Lr@JXE)vCU`(gUm z(=gfJ0jQ4-m_{EauYSxWGgaNeA}9e??3pC!xw_))A7kKso;Gh^v08AiH^R%l@hrVW z4hv=nvXl86h~7LaX7MeVyx%rLn0DS9?)!LS@VeKc_9k6e(JvUbd~d{Gd$gFHO$xEp z_X58bRdRE4BpEuR2u3Q&V_xMg@^5r5>-kPe{e-2MY{vj+di>i7nWM_`t>V zDwGR0V%84>nD|8wgJ0ReE5AfGbk`wraIhw|U$6xG&(XppixGIaD_i(a+QRmFeH6d+ zxh-0pZeR!B3}(i4NOr7QBPtHBhJxl<=<(n(d$uc{3{BSnsqslxFrJfrztyPpYAv=c zHsQwRitw%65Bm7K!*L}mdV5Je8`|d-ytLbgGh%J%G|!!wcXcWAAzoxkZ6Dj9I{q{wi*!>bLui7vzH^jCH zC-G^op`_XT2^z6y5EK@Rx!FOaEU5%;b$%x%7h+*Z?*K@b$ieKfAz0=R!QOP2;Q-Ss z_^A0XPO^~s!_=$c?1}_PJ&-DMTztlRFHZc&Ak8L0B-f)sQ21G%9xjWU@y8;W968Lg`b)MHP%atVggXh;pRO{df znm9s@?he~S6_e-FKmOlgy8T*`A5;lptK0GA6M^d<(P4MH&SA~Y2=V=Wd;WFHHrid) zpY9rX4xM%;K)TFQddu<=8*1gx0~D6?e>`LG_5MD*^_&2Hmu;wV^L?JR`6X(+y#$v> z-G@g#3NW#MF}c%_4gM?CaEPNZ$UWVH>2qwU-QQcmc)S{IT~Lpa8sR)}X$oFW`ih0_ z&&lK#b-HS=8M)kWU-atr#=p>=p2#ReRok89kB=W4eDNQM*}E90FHfNL8&ja+_aV4k zCCfdO*F&>jCdRJni{{O?;P6rjV$`?s#y!vQmO+Kgf4mT2#1(#Wpn!_bz!neR+h^Q=^=DrtS_N_ir@$C&OIE zvlYP7+;KE2JPBG%4e9>0f%JEF0WybFZePylgt7B;h6y{J9@L@?v~U*JBoYFvZ6JmQ{Zf5gJhGVfcBT%(QNf7;`U@6-Yk{P z_F)%L)wwU^7577rUy)QxZ8ZN)>q&|6VCw8u1{>9~@!M+|w{u`845E=RIrjiuiWx** zQUb7h+##G$ycaXyd(u1$6>!s%joxmR*z39#jUH@-WFy(oWnBP814ThDQ=h+C?aM>X zMsq#uA^20qG7cG1$IHpi~hI86%F&x;I~JK*c`0S2a_}G=D?@YnzNC(r0){? Szbk?mC0{?^)k&*YkNF>V#md$I literal 0 HcmV?d00001 diff --git a/behaviors/custom/Dribble/dribble_R4.pkl b/behaviors/custom/Dribble/dribble_R4.pkl new file mode 100644 index 0000000000000000000000000000000000000000..781d55ac9722c226ff719f5700b4e48633d4d87e GIT binary patch literal 24148 zcmX_nc~nkc)PG5HN=1_*D$P_<8lJoNBN|MFM5qiYd`pqY*gViYAcayQB2+Z!x%)h2 z$ShOlOqoNZ4AHOmeb@Tkb=Upl-gDNvYp;FI-k;&b4BqA8FY&*ZU!KXpoi=<;WN7%N zam&Njtr!;?8M1!W(sk>WZpuqj3Rt&ddDxnW_3I*+ug}ZPOOyWJfq7}NYXbjoNN(PA ziNrh?$z^%Td2{kirb|x8#JrW#Y0`n~H-)eGpDBZaZ1Z-c{NGopG)d>YS+izM>mRuO z_c2{!ecrMilO-f2B%JDE_#=tNz%2=({P+H%cHajgi{?Do?c+$BH*JIQl0mTNLL5A> z-T+PS&v0cTeX^6?L>;fMHvaQM{@|vuAn!XGRtQ&+e*7?g_ul}d}+n@3pBJY=JTrO`Q5-SI< zl>MK8u9W}(;ha7rDQ~6fK!|2YNF+2$;+vJWd{|c^jxiLHZ-6Je`^12CRj+4S?N516 zxBi(_F1=Sfx0P^?t$Dk@37&3(KN*k>pv>6hv_apWqd=y@Fju5Y3vRcCRvnkK+Z zCE?G|Ur_V%KCfrD5ORMQ!-Ru{Z1k^uY)aY!B9vpLSs**zM@QGB^cM<+{yYVe86R_)T9X&Egr&kkPL1|4eWJU~!H#4Vz?~pxg zd445aDllS)en)V|qC)B$;l(!IHfYq?eie2qXTu=gAKn{OW2*c;I`=tPBx7dJ_6uA1 zQS!H7Vg5qi`@9@2b8O{j_e)anp_`1eyanqc4w19k5qfEtLytvkiGs;ZkE3S zTJ#;@wG@-l?^ySEQ@n3JoFpe& zu@j*;1hTijpj71}?#-xBb}Cz0yw`gW%h651nRmyb@>5;rR%<5KcrS|wA)YH6{JQby z??k4ko=(^H&Za1@Wi%^(9TkhcXi@4fzO${8v)E(6GT(AAJAOl>!Se-dR?$z9;7T9N z9-j>l-5n|1+yJhWUZew8zj72P!;d;_Lm&5+ie4V*GMO$Xw!;*LHpfWMew6oHn-bA?s@s_1dpiyV7aQsXWq_LEzT$+;A!Q8d1cRuyM@mku-ZU(wQCZ z$zYoAY|-mdBTf4{l6mnCc=LG&<+#nkMN`JIL)qV9XvjEL@$e|ipPq`dcFYBP;dTld zd=!?P?WB@Vulc*K?NFFlCn#<31^(L$im&peSWw`%jeRUS{I*^2siDSMOSFiUtvt^z zTAt%V7967Zg}%(g-WQTk|-STmk?n79{6j_u-1q%v#gpChI3X|{YU}(9SxJ+OqG}|;& z;1+JfzQoML;Smd1Y|XnY*Cm^A0D;`e#xVBVopHhID;Zt;qz+`|MX_i}WifU=7WMJ>;r}bHh zShL3+Zd*L0kwHpg&5a2-c&;ByQD|b}N2lZ6g2U{v{TCWD+)DVW(}S698p*CITa&>^ zZQ8Uh6Cx!aWB01HSovTTH4mT3PdEDo-zt1i%`XphlFA_Sb~d;vCqvQG;p}GnNiH^N zJ}ag{Jn~+ciH~Ta`~hy#{i}RvUpr_Br?5?bK0=i4Q*Qd1I>?&n!RD04@K2+{*zi6l z!8}<7=NSfj;XaQwE=UV)Wm*k%ZoR@QDDwb zl3{pD3%c~qMf-GP+^95zPW$HZ!TWDfPwfLJALh*pN9aRE*L$8TNP(_*;q?6IS-z~m z1(J>_vSaeA+4qMQl($St)bdUd=DQCQRdXl*cCRFRP<|7v`VXK;Q4I~gjs@}h9xxnZ$hJM|g8S!9 z=+p9G5_`<2MJIJ=U*Bjn3%&ry5<0;3k`gA2mu97QO)$1z7G31O^QyZC!_0!E(5H|l z82QJM`*D0d)HG%C6{i;pntNLShFs+OuJ&-{FPyOOo(uZSxd>f?x6E@~D6DJJK%I6W z8l9R%pOW%veSsg7C^ByPXgQP1wk#Lq9sJHLQ6jj=wuEhk~24xSm;Jtdsu2 zH=sym1;1_g2Hw>wUa@=02RZu#-K-WjNY4orX1^rN7D%Tz;|_r@#LJzqs{2 zoBiw(&iA}b$4sht2b&e5jz8AqF3yIrhKZo^CKfuLWkF+N0aF_l4bj7fFw^-HS!Q?& zuC{fi)t)CPYC$}G+jInW+-(=wj@S$nt83t*DGxEXTS-3nAFb*8A_yEN0blRra6Ozl z$^~fP<2iwpp&=<8+Vh(hta$+i0rAjV7ReMJIZ;-=jCjz$da+bRAiWq7M6)#BvedLU z+)^b))}o;Uf92lMsZU-Qu2aN)3LS`pAF|`ts<!!Kk+z?$H>g0mpv3(AXF_O% z3vRy?bFO3j1lAw_@7$H7PmnEngGFrd$HcSx&{8N(Rgo{bWrf+C=Z+)@-mwCPNbG=c z8wW^s&BaHzmeKOSZZPieYn<<;$ZDMqQr@HnGCGpW#+^{2PwNA*`utt+&AUm*f=_bB z|Aq3LVLMzbyuv$4NYbea5g!tn$IXsU;4-%xi9g!KK+9-Z(ct|X&?0gKR~K^-kKrNO z2}jT)?;0Kn8c(90O7N(iM_MlwAoyDXP)8$pkK{$i`0@B9>ICz@HX1CPrnAq_Q{eWZ zHQc9XwYdTbgH=^ab+2nk) z0wNn?>A!v<-2HkOMkeeBrMuf;bn14{y*ZihllNgaudC6P_#1R}NIiG_5}PI|y0f1UD4bIIW#VwD&<4KTX;L%%8M!cQ0i)e;jx}X@~*c9A-j!V;+g} zzHyX%ahq`Mw##fr(*fGt^AOVa=CX~W9mLP)y%5LI1olzl1KpCI!+$t+kcSFOh96_# zf!1@Vc~Z=@ABUs4%0RAdbY`kcazq7z&$vsgp0n@D7HB<3gt!@;)wCd+0cibhDPbzfn-w_rv)QJj5Z-U|}i6H-R57_-T8Mc0} zon%a#cSni%uamKRMH|gDvcOu&zZifL!j{`apZ^;oY}7h{ z85L2Ky~+`z9u5`veU+({vB97TqXX9+m+oZI4irY9$-K&e-D<;iUMg|64ZOhRq?tFimtFbwGWT1K8;J~pR}{X^-$-IJ+n?iki3pGs18 z%Jeg70#(^eVJbG)xG7&u*u{gT>~4-4oqVzb($O+ug;Ev-@zm|Ta3`i3*PUxcmC@tz znU=9YFxQ5KbuD6HlauhK1B1;LN9oqX!|eE6O)^ssVSMZT>P+Iy3nD*9l+^|{`W_ji{s$6ho zc}gcKnTsLI&$jgGRy=*akq1hlYV7aBB7E#K2}zD{$vmr`Aw4dUOOGz?&q*y_U7om z^%?glI}uN;It#kizqzpi{xo#>P3jFD&jQOQvgSRjq4w)5ihd#u!eRSxPu31rusDUw zpXA1c%c{^+Zx>p(FbDf3yNOKoFtfOhe|+*ju2xTgl+-n(VwA=I7u!Ksb|NkDA4FU7 z`&e|?8|I{NA0&K(`5~&=sHt@m?*u6d*N*Oo>KPMo_SPywzXrH)@-}byIhosYvxIis zGsHJuvO=ZM?aWs`hSjB5GM8O0PM5i2t?b@Z=D z)@K8sJK=!y+XqIXCBFjMSgsT!RPEsJF*TTZ>nvC7F_(>s>ZX<9Mc~|K$yT+NbJlyZ z$uj;rB%b!-|NJ%++{ztC$CF|N7GYWZs%t;^m_Z0rA`-w+X%ASNC}U*sI3`wLuz!=8 zaK3&UD7VKk*RLvU&&7edlwHSNE3%3+QnjDL(vD*7T1BCTwEK+cOO`)V?5ZuWz0tvlR@^iPrnP21iLnlV?&vI>8 z$_NMgR;fTAz6@qH2W|Otsy@i?3xoLDZBUuui$lvYxTUk6a_4PL@CIkeH;olJt%*4& z+U@xHT&eOg(E6vyHg+UqzG*j3Z<&X`ViLKsc5UHyJ4sl#fBl)&gBV|Kc9Dva-aN11U=&fMM{_EaJa z7te{sC&zwr290h^MR15^jptDz!k2Xxd$GImb}YEyBrY4suhiGyXqmwfR9(4=JGWbz z725=1-`{9DcT|})76rf(!&Cf*ED`HWT?3_ErGlKNA9=gHDyR&Mz~QDt;9Q9@jDBJX zQzU+Ht6v^sTyiiv+>pW9CErDvGcM5g)IvBCZYF98aEA7r(d_yiUH0)`EWMWFP^O@m z&P?e7Hx*aTNaGOfRn~<~(wa~)W;wWw^MTQ(>#*?JL0r@2!VVwHW-Sj#h-1x@XvKAD z@R*PzR$fufE@n^Qwi&0e;UV|9K_7;)F_Q5V?qEu<52xZ|y9y{Zki+}3$Jl7U+3em{ zcW}`>!d_LUQoNcOPHp;0ku#*k6%(|?W6e&pG5e;oUnS~7{WDU`7Dae?zZNtZEW+qv zYV7%judJ}%7ZRQYV2^klTW|OSx1X(Z&fM{oZ{Ft*Dqe>ChmYpmk!3O<>V1K4=MNHJ zcME|1%hmDyN>d7rUc&l!r;_K{ne-&ZX8=FWrtOteP|tH3J}~iTl{Ti3{dNrA_#i}M zy*SigXitTAPJ@B^8D_A~oEpSZI7&u}&(#uvRk#k*=}>~E6UyAyW2KzT6eJbZt^5Up z>3H_^OtOzZLL2vgq$@+7)AgzeOt(A>y-y{O&XlXMSpo+K780lD#}Nuuj*NlU}*BnPfK{Q#O5)=lrcWwu`tzn^{ zbUhPfuLn`=;lHAS*Gu_b`}`^Nx>ajW7(ph8nTd024NXKSOrXBHHx!h}4 zsNj|~`>;DlB$+UmN-nk2SBEW3vuHhAfAa&GO?b;4b_v10u`9SQ=^L@LFc%jrdQbc8 z)41?KYS_nZVJB1-iPQw_uKIPhA*>KLhYrH!zu)n-(v5ujMHyD}@Gy24=HOiG*v6Xq zt=z2R)u>_;z)Ve%O#DZYgqbAW^;M#W@tJt9eG8&6p2@V?vi$ls-q<-FUKBr}asKVx zx4GTaio4+SiFmZX<&0v7NZJ_unpgR&$04#NNtEx)V7M-rbqV zhiTccPn+hzGqv~Ju9P0MW;-e8eH@%W)QnS>B@0Tbuks(bMn2?azewGZjk&R#Yj zI6a5!{8*)9e4&aFWkRQcNbD=jp{Gfs zIJ40WY~yAFvHHq(_FJJ=G~#O~_vWQ4w4Hr{hPRI+OBgQvS;b*eVgXTp3z^nR(Min@ z5Pfkci^_Bb56SiHzvF|1+R_T*0J$hinmqz<#T@|k{#tfrS_*4yUxz8N_24moC%o%c zfGrVbT%p<(T4Zw)1Fa6S%xj(Wux~6%JKTdSFF%lSt|8v_Xa|o)()`^&+T77?2(?{4 z{5vffPDS{Llj|!JNov@0Gb=n&7#a*2Cy`=?FQQ#IvA+2Nah!3Ll#I zv8bHUIBkqFcAcL~28F$_@qIb=8eHdpY-#77T|detKeVDBZx!g^$wGGiW*q-wq7b-# zRqAdS$)s(p@v!nsr*4Nd$Wv4j&f5Ep@4TAGS&nyOVRNXXa`pE(Kb*PPHOZ`QQovt`O zdJ-6^xnRS)P}a{I!ky4k(7!#B{Z&}UB>j0ve)kIs+@siKpF1QuVHaFb6;VV&JU=eg zVF1$_;fH_@iky0kf=<;_nX4zZ`M#uJt^}q;x`Rsac32Zo0mYZT!HcEpEYf8bxNUNw zal#0etNaQsc*e8gwy){t$^%Ku!ol&*RJI2I4gTXKPG&Pe5w zGc>6sqM!e>C2A)V)!h&1E=@OQRX#e_Nrfm=BiQnQ|}v>BDyHM7JJ5UiBnE4LQJhW*>vRy(!oVf$REIHpRI?d&@<-XEsnuXR zqQCJ+tR?6=KBafZzKOJ@^|0@P18q01#J$bO$-4Iie-gj2i>ivdQ!0@j|OEj((-c&gSQ^g6RXi?~o_lKKVBgm@3Jp|FdMK z2M4(BBl2uU{ymua$OEGf{N~Mf>(RH%aa5sOK+>c8>3l&Z%$0u1#Z;#AN&75?k3Y!@ z?|a8`wzC4Dy{sHRtba_4^Fuh%yqDmxb0U7~{YH1B3`kkhiDlVcIfa=%l@$Q;SqA2&-w5UUt!X8;sf50Wa&P#^|#f7p$m-Q_6>NTb`E6w>)mK-cL zl>tR^1kqkkW;{)wMSUB`g8wK8moBS>^@rkER9OkM1s1~FZ$Id$QVIkGyTMO)OK5cr z2Gi^=Jn`lL(2=JDIc*5+u>@V7FqOqzF%kNtX0caBPgwom7_b=HKp~yS=u^rWa9Qx0 zT#Od+E0w&dHK?9=9ewtD*d12doyI0g#tT%#H1PH~MVz|p423;!V!=aI#Dmm+^HEX@ zgMNRGnYk)K5=~H)6&XH*7di zBB*vg19z~MdKNiSa+we4-PS@it$bD}YY&abUV)475?*S$&ICVtc=crxWYwoaj{R&+EB9rHf(mpxY;Y&&z0(Ae?^5oSicOv{`a0M z`)I-J{Pii{Z@~W0oC#lkdokPn0W7=80iEwul5*z+R%c&FNB<4g(9I;+K$Fz7~Bmp7CCWzRajRfZ2b3&i?Aj zLwAThzp8RKyC882D_qZ^ZmR@#_^xB0RTWuH_gL(;4TEs!BRFyKdcN=GLN*~NtYO`p zMR3GKozjCBz_UJM&frA^iwbPxr0jOVh<~%#;C?msCrFY8uNqFpG8%MhtTX9Oxe6B_ zHqe?&yXfGWGMtw!P2%N0VU@3h&|?K+7?_~IeY?o3ne zd_XfBm)Zn1OVZi7z9~@9>MR^`!3Tuf&o+;^g6&B1#rv<+(Dtkw zDdgC|_pzB&7J87}Mkoodgs1YFcU{PStvfh>%_q&_wOn4wK^V0>h%zO5X;eofYYkZd zpCqSolb77%Lj2`mea&R(i{8(UJSt?7S|Xg$+fL6r2ekFT4B9lpk~=<->mf&WQOS~a zzS49FH~GRAdTR54H~xGOg3`j#t4)ik%n`zWiyWa~)Dc)*R|H#9EaB@=DN2ZShH(Sf zMD!&eRx4_OXOJ9kZ+t< zm?ieX6L!@6H52fI&!V)MyRL>6^X~RCRIK-Ciloez3G&IAlbfdQOth_ zU~kxF@XmTew+z>C`#)%d#L(&7Fa9v;qzVUWc|Lt$8eL4SaTYHLA_`VX9iM z`PWy7J(T-PeSJOn-#!WW8`4CVM`nWRu8E}fzC$$q)D~Rrri&Z;w83xhSHZ*qPm>gS za!1Y&=Gf+m0&m$E=U2@!nB^7(k-@WImQo3;uC&CS74;ymw3}3C%Yp7C1}e#q!QaJ= zyEASL*D^Vk{4##iB1h0(;tY46TB* z;FN{xQtD<>>|BoFF|6C3d6|H z_{t>)Cwn&0jEh+~QgIm;?mEG3&~pIGzEm#!#VnShcZkmJ<1oup8Y0&;Q*^5t1SvJp z?XBY*hgIz1qPt@#_EQ!2?r!FQCk$zX(olLM=SDU1D@pl^3YsRUVUW~qn(j1=zjaEP z-HIq8jhC5N-fe+af>FFurJZ2>r>SgDZ7HwhrHZ?v-f*hF@6nyD-5_@(gP-tGhqi(- z7xMCjV55l!M*0Op$L4bCHvdBRmnh()5E-UXYbb8NHAlGaoTKoY%{K0b0^-o;L&Qgo z<}+QX06655O{}$YKvSN=7MUA}&*d75!15X0vg?6=OS$FwVTP=XP#uW*W%#K z%gZ!;<9%L!!7jdV$O8KJvy8tgNPrPKhv@Ka3vq`@G&gyn9rT~_;JHb=alyv6lkOh`IXDVefD)%=z3!n+p}# zY`a~YPFpm8Z@fM?EXEcou~u*`EQafvx1Zk`ti&Z`#PKmEm-xAh7cwSah&2;(*f?`< zyfqovf7TcIbKiQ&(cF=j{5g^RdwvSK{NBQghH2b7#|CIL|1MfSf$_Vl zP)Oz4_1B?^>>NS*s4}#9{fGS&y+L)!r~GD_rD!F;hF6=nliiQj602>!Hc;oUU}JRR z08gF^Z#)ljFo;OyPnE#L_k+riW$mP70Q#hkgjgd8{f+uWJIAPfMBQ{=w{s^*M50 z>_lqIHEGPTTyCLd7|e~A<1gmj=NC`=%qi#@L2+#r`%il+ByZY4qrCHRc)L7E2X13= zNqO{AOPX#L#fnqTYg5ns9o!D7X6Bu})3$2a&IDcz>Jf<{;^_9DD=HveH4;BYf z*;ifAvMUC=$(H=Ao$9zYBL^KPDnMg?752|{=AIla|`i6-9mb0uGQH;mT{s^sr0-Q$krY2k?5qhV>xc;*sl0rxk& zfJe^A-c9O(E5!oPy)m1m-yBTaPo&^7zZ0aJ;R~VmJ>1>IaopKyH+hTnHo+N97aEh0 zMA_9#*g^NzXvD>`AGZRja@bWC`ZEsyw%MUt)I6$Mbx=HQp(;Po$U_iD65>%(#Y}kC zhVP#Cir2cDgZdX9v1cC+;@qxud=}Ww^gb*_s2D0)9ycG+V?mEhgoEd#V3oopR0}SIk!c*HPPqsYZ)|9S%LTe& zk`KmD@<`)W8>n|lqnfT8`{?$E78eF{wnY{XpjoX$o9g`UPFC&Hh`dSY?lj||R zE{&~R@dM%t$Ah(38yC`Ymupmf!EacnLwx*LjCGg6&BGknzK?4`>%0wc7Mf_cWeGbU zcMUCmYcf~Z2{V!<;K$U5nADjL6Uvn7&}kbw{U#1f-f6OtSdSZ`H-Xc`L`wInI6vTbj{|2lNmKgT_G>Jn+^s=}Kwmmmu!dWunvvGNVGG{?w zYZSgN&!z=;4p3WH06n*hr4`&}`r=xRbvI_x>lKIa)Q(f!tz;ScswRyl#U_+iR zicB``JYAO7X49>^s9MH}sMd_^&U;ghZr_0J6T;K!_59NL*ZI?1dik3l#Qe9>*3j=< zPu+Y~W6hv<9A6j0(w=GIyPd0rw>RI$^2#>wabJf{H>%l-1B*ezFo85~ARls6#O4oQ zz>IBwK*AD9_I|4xh>mO)g+Hy}YO5QZy(eW-=(ZrHI?F`pJ-W&{>U|fuNE~E))b3$( zh8(UC=JM9TVp@1am)`+bV9AHOq(1o=?pE@^qId~$c});Y>$-ze^tAA}Xc%4Gl1&N6 z)al3ia!!BvRxD5Mfw2=m^X{J+=Nje>2S02gdKV+|EPNx%eQUyB_MOBhW#zEd8*A9) zUI}r1#0;EOYKqWSPTW1DZ^j?MzC4B9*-%9j#{Ht{*K$!j$qOQaH-o0d0{mo`401zV z`3qgUdApBWML#2IxSL1+;RYRqa~FLnI_eXJoc#;D_CKsS`T|Rg8c}!67@XKW7Ur%h z28EfmaM@rCrtW+}du-S6x66!Br+y}L=zoLC!II*(2|Bp+MJ=awu8}UaeBkOw7tr$Z z%{aoxkqwDF1gHJQwBWoS>wThzEjEMr+XMX5MX3agwTr|121eY8&NAjDw*=~@OR;So zi=Z|BEk>;SO|w_;!m5->@ZM$&6SFuvW44-`s?$JA3hxcn(|QUq&Z4OYl5u?J19+5n zihH!MO_1EaMzH#w2^VL*5A~Y2PDD?&|*)os-Pr+Ro zi6XuU;PRp{rnPsZQ1hGu4A{ZRLHh`)uL?)A$5u>JV1a9wgtBax3OqMtfFp6WV4+(3 zAXe`ktJbcYcaZ!XL&4n$AyJ6uoNTTFTuLMB3euFOz^`&vH@ zrcYi9-w!I{ok?{N;53YN+&_zfM@yJ8wd3C=MR7x;j(FjYi=a5|DivpMqm092*cyov zxFj@V!}X*2kr#Bhp7w5_8Q02SM|s_?TgcSZ1SSMEyF|NEz+9ljGVxzv#!8vo$cM#VwwGZFY?E~ZZ-WsuvtkB+NE;O7@z?7?C;{-6tw z{U?U7kfJT@`93)eNgct?tE-{^@fd7LH(?qvm0Yd-G2AITpp$*HVIFF$A+OPul^htr z#m=40QYrDwPb^^Xv940)e63jZi)mYou;oy>SgqHgMgMWTypuAUtU8ohq z`3}Jf{yZ$5WZ)&P%lI z(mnpF=N-21R1rM(yn-!DblDEm2wbH5jLc>n!nB{0Sk9MGwCl<)G)XVVsGRQ*T9L)x ztTh)V?-BENmzXn|-WCkz4|DUrcwtrJde+ogz2SOr6!`Es&7ngZ`T=CZILPqMiCjvM(jnQ5wyVUL&Y<6V|sY&>akLG(l^ zQWSS93*~xMFnIbDirC=HT%)u2Jw@|@jmQP#L+3H;;yeD8@B^uAj>nh*J#kQ#E{45K zhwnc^DCg3S#^{$LxY8ekIOz*7_~Yw(`5ILhY^_dVhO^gT^5s7;S7kcJ_WxyAq9v?+ zYAUqTw-Vn8c7%Of6|wtl3b>9@r~1FO+&_T{%^N+Ro4joZ7Mqm-D$1bG7+XkG^5?P_ z%kuJatz6^P=c3tZDcqcD8^O6MC0<}(%^U2Dr=fn@EV*Zv;MboapcK;xMPYJ$)~~6Y z`b`NqqB*cP7%Q-X&x3_=6;Eh#QVo3gT||2P4N-04Z9(Mt7*XVPJzB%BWpN&lVcY&d zXb8ySWIqp~NvsnxB>$r&WiAxmy_sCpovF7-jmw`q&<7`$Lsg|LJF{p6$z^OOadsOQ zd}b!SnJCLUoSsYn9Lu=fn`^0MSEcjF163mRt%Eqnh)bfnyFv&ba+|xe${t>i(}Uk5 zcz#1*8vmxhm07Jy!ss)8kZk;uPM$M`#j9n-RSzdLg)eiU=MsmB1}T%S%>sUTZ=7i6 z4*>aA}uu=6%xTkOo9m!eCK16A7 zshRDV^IvH4F`#Knqk+)TeUFl^l$t^Ek; z2Z-NMI*lJ%vz88t3`i%&n;uv#qSfycN#db^`Sot39gBytieYO|+i4Umanfdnee$?D zXgY4Y(nf0}2D2u81MGNf$Uby!ryqI^+|*fF_~GCRE+TUj^=x=3GVuHeil@Iq^01rW zJ$f9wX?_lJ+n=(!H8+`_i9262e--|?yC1n^G2>-Ef|g1HNVrs@f0;4%UK_$TmJ$hG zG_WB}I^xkTN0|7G51WqV6r}J8OpN95$g6|+d_Wfql5T{#U(Mj;fe`53>o$N*A2JMH zf*EV8xmvYoG&K4_*EfvCeV61g^?59dPR|6>6LKgUo{n#74N-5h3R8dQNZ$;90#n@q zoe5jWYSet%5IU6&Ytv>rADiJ(uqs;gBr`3WGi*r0!1>BW=cwf9OB&hGP18>G^A&#v zxJOBU(7ZeyUCqt#s&@~}`kRifHAS#N*PB&77>b4QB1*1qV8M@V;F`l>>{%MjE}hy) zgH|GU@=?5?H#!`;uf@Wc(=%XLq8&?7Z{;)s8~KjVM>y16QmoY9izRzec*Sg!m@GB0 zZMQLcSDT~t-$j@|@(E1&yIU04n$9eHZ!<4*hSBfp*>aP=P%JqTFP&SCnuGeGrR*`h z|1g@3`zME!Zj8c%+P5)a|5&y>M4$br9)WHO=kR-kKFS|;!|I1?*izpjR@0QhK6k9; z(po)Pg=jsdl*!Y)0gMp(rH#)T=K@n6ui>*N{T0+Kej(U&RgG%hPx60`bn{n+<*+OI zO?3Z`48KD8uBdqGW!^OLNMn0+E8o{p%`O?1();1(;aBf2&f4!3bZY%4iaDsmR_VTg z^^ZFEFS7CQZ`^nkEs5ag>~g|)3LyxuRWYSXo`2-&$M6502cG7YFsfQYoZDoJ{k9&= zqqztj=Cz{RR%f=_yBBZ2))qP`Yw`I@#}DB31>D8NGTyXh5FDGV#3J)5;Qqtm7&N|v z!WSj8lwDKVKV61mT5lyy{#Jxd(}uP+$0ee;B*`z8=_zSi-tI9-%4 z>vlf+&xf08+sU0-p3b$0zsA+63G^UTLwM}Pd$xRR1>0CW9PA|Ha8v$l_8|W_ZvFR> ztmZG^_NeF3W$OVf`ZtLl@%Pmt~~4;TPMx$Bz{)@?{d$5$Giw1$~dK zu>JZsJngOpC+6MB{FalqDLhIS+^hoGI?E#RyK|PSzV-trbj)Wb z9~g2z=kGf2J5u0mkbBx$)@L5huKC6tR4c&YCk)w9nL+qH*A}w|KY$eljhu8@F}bhW z$L6UW=c5KYF=vaiIowT+GH9Q3L)!g-TAkE*@1DWT97MV}c1(F zr(&EUI4F8K;Q7qtAH_VQ?T_4;a|JaPtVxH><_n?ZEDTsPG~wngMf{t3h4WPU0rD+z zY}vR{);Pe`KF(dsN-LU}y~h-A-L@TG{Ar|@gVkZ2n-(V_^^q3Lx(`>{Tv29rHcd=R zrgM`9v#Qk!O!PaR<>~oxiw4VpxN_%Vs+l;PN`LaFC5l&gZf+HQl=m;VzQ01=o2T-y#ueo3 zvZq{je-=-^W=-d60PAPW8Q*q1S&gaEEr7j-x*E*5V>_kZ0 z-NbGvM00x0nb)jdh`ST?sqGChA64@V$Bj25NB)f^zFvN$=K6vw*6!E&HI+aNyHrUv z8y3+2lqSJSBR?`&rWOt+7XWq#(57YWpl^}{#rHGN)7pfuFp;3AoE2!WSq(K`bf5X< zDq^|hFBY7j%Kn7gQuoX~Ok_45^t~0S&T_rFrX|(XWlj!#arGZIQtB4nlo3qMcW1J8 z=@;0n*uY=j*-GX5_dxhJHJtz6g{QoZrFP@}Y46D4^uo_D{as6wrd)=ebP=`-=GBp?QOzt zF)MOJTZB@!GIa+AoZ!6pdz;aL>?t2!ce_=Jz6S?7p}?yytZkxYP(u$+d$*8ZQW%0? zZ>*#(juN=GNE*Lf4Wy~J6)l6qKhZ9$LzteIjsv!H&?vGJ@(a?bN$3F{@^%3~{;8Lr zKRg1BzGc9^Mhp0SL7ipVCi31v!FS#torThxF(#YRtd-oS_C29ufWH zhLqO(U{sYXZY;>;+YI$dgU&2+?!`uYUNjloQUmEuooW2I#t*pUvVafhB%&s{%?i%w z(pM?X)S*{N+;1_kHK+uR;6xym@1}W9)mrRqQsnt@{ck zsT{^Oh`3(FCX{-5470ADh7&UFOl5^DZqiJ{FSl~g*sqg>l)i$5iYV4E<_(p{%*GA2 zaj^Zg`Kz9{iRJj^a&@!qUTWm*QaRH>pt?UIDo&p zf&9s$AF#W544r(h942uU*fHD+ZL`&|K)f%Cv)6(c76xHrKH={jci62iMLaK9G8^-9 zexz|VbZMlcN_v;@(6)q+8KX!;GG(~PWdkK|;(5@GP;%hm9#|H4nXWGG#jA$~i0|@9 zcBJhrS4xeeH3ysM6zMNQQ|ELVUHKXEN<#QcYgd?kQy)|hII(%Z!>L9|7Tx>m8!JxE zMfU|Wcy8<|n7cQTE^hPVpAH7&kT!cZx9K*`xfRJZrLrK=&>Plf$WbrlZRk$&tv zcLaX7c?L3t0C>6||2eJ0r>j=fZtpi{JM?Vf-_A6u?ZfGYzEbLQc-YJqI#MDThF>3vxl&?&`R!567^jJP-;zOoIKq{@9Nv3#_7(pvDwz51 zHmLk7$sMQ0k_E6^u+sLia6EfW6cTK}Je(c*ujyNPZ`on6{p`toEakYp#U+^ZAd)XC zFGDB!0DPYv&nqvuQE%gPyk@qAkA1iVFS|Wpc62Lt?9RhsITozWtY4^6s;pWX%&>h)wH`2Se~!^M5Ixx3xO?{5lgiE$9A%3LJd zI=WFaT9r?83FOt02{>(6EDo)UBb`NeiQnIg%q=47vu%{Yr0st z(?c;69)h>dThZnV#aFy{#PQpsV?pbPLY-z$DXkf{neUo+hdQJTwOpn2f$tr=5I3f4 zQ-|S;aaMCUsXp3COK(;18y8*(PhT$rm!fj0oZ*OfuT8>($Mu-=!fT@C$8Ry6fD#;i zr3B0J!nwJ^F#4`*2}EvD5$laEfl9(=RvvZ+>YBH)8MA}%_L+SsUtlCUBB2Fs8+}P# z?H`fbjg#Q-dKFw$3gM_@Avi5LMk03Z;)MaLX#aNc{MRuSHLpi-`_uZ|W4#|*?3-UV z%_fKFbe_N`I`^=nYmmrvKpM1;D#NM13-rQhGdi>74vvwR=Qp_4pxJf1SJ~2q?bmVb`nS~X)J^I#DaUf&!zkD){S%Y? zhx7JzR`mAZQf{zx0$=l?f|{<6;;zq{ahgdl$g+6i)bSMj&brW&(zW!YhdH+2ErTNx z<-GcQBiOhFSv-3>o1_))CnbrA&CkBIMaaH~kJZM-&54JOY)wz{TT>MQ`+cg5ESE=L0QB$auP{q^| zq@bqqI39fVnN2@f!tw^MqXQE5yk5q{a{i|T{Ks31?Rr(r^E@X}RS#SA01apgmbKJf zZN7xUFsa1!L| zo;dW8E`JHgnLA9h?U5?^7}yUFKTn`%M!(0_Q7ia?5##B4Wes|ZkL7QN2GIXJTKUSV zHOPXJVeI#hxXC<~>U3G)m!&o2@wyxu_4+)Ru8QEt9bUqecM)7~>;()`5qk+{i@D5Q z3gG^{7#{B^q*fiV)OQnzS>?fXNii~(SaY;Qaz3_ZV0P4H5UI)$$?7 zSep7qe&gN&6?}XB8LIL!94+RB;iyl0!RJCW%-%B}l)SIey<86X6S;?uKj#fZ?KQ`uU{E zPQr4oeK4*6;7>DOui&0lwsiw1eJ#gi%AtY!CO*FH6qE$bp#NVH7CqQ^o5!vMXR){K zf7`;8#p3<;O;tN+$vt6{GTo|Wo!CBE;3~aiQXD@=GZ%t?BAIJPBwOIvebI= z>4P0J^?b%$w|yu5Zw@fp9E>e?n}}m*uEou+k8`I?&nBzi6bTn~CXqAurm=Izi%HM0 zwd_N?I2)!uXX`wA1%v0x#JES7#4q1vVN$Najujqf!LuU-leJ~SxKmle<6)B^pn4H8 zxwl1>w7`(n>poz|$~Lg%p8aH*ZoG&en~d#C1W{jdx@crxyJ)lHZIM<(8VUcH$vk^1 zq2qBSoBO&!^ohmegUcmMas$D#8}r$kf0EgB?I~>QykcSF$}PmeGFW8s%>;z=>si;? z7WULXRoGhV$z&xo$Q5SL*rm__45V9NxL>IN9;kZGpXi?32m|!{?P6!#oHNyh(Q{9vV zI&?DoqF&*sh9}H-$z)GgWiV%(7^a&t0<#0n*|*0Nx$z8Va-&;^$Qj16M#E;|qvTZb z@CGBs|3klJjgiKc_t4 z&Rmd^$;4DA9d`EpEmC}GJ8S82X0PVFAk_t5MecEtEaim*Dx5&!$610iS8ig1D!ti- zohpK7ojH@Nb+=IN7GP0?A$hLi$l8ZW!WiYBM0(sd;_MVfYO8BlMPDepsNuy19jhTJ z#WUC%-@PR1zrUg(MXD_R!Bs+1+*zZ!6UlS%z48@LvV)JLaX&vLeAP~8GDUZVNAI$P zJ!Xb5Xv#&A{M7;&YI9FC+%r|!mN8n0@kt~f{<^U=t5{}K7{~gSXR*fpQ6lq0=ZI;C z5>u=@W-;-z=uy z3lW*8PK2XI+u`8LE@A!90W!?6kV%YO2oE&v$e@3{$h#XJSiL_PYERxHWAE#UcPN*c zT-imD$BSH{#s04F*r1DOJpIkOz9-@KsLRBsznYodjAgqE#*m*wPQhcQg`IbQGfAyk zg1m}tZSL+G_HpVG5-<8rhF=%pP3{g%Ik6me1T|vlXKNUfKL~oyC^Em^f8d|@vZ%4k z7p+4@qLVMylf>M=tUK3>+1d=p>ycHEshvoaR|k^0ON|K=EhJN4N#pXnhsh(+B32mm zTT~&X49n)3;nbGLQ8k?TX^7A0G&>=9u`@X~ph{dUFlOI|E%5Tgm*@(tO?E4ko{^mPl+8h*H{Q(yo6+IIt#~Ef}07T3Iaxn^cG4 z`tUH)@H`yvUk?!G7@5N2ak@DBXA}##vw&vQK4J%@7ocBDfDq9c&A$0l+|l)2)IHrm zlsQstz^m9ue*TJt&y%?@pmJDPbRdN^_dX`2MicQw)D~K1r@;3v4hF{?w(QlvEohkU ziyuafM9s_}@K|jshVZdqGNe_^9D1M%iNRg=k<9qjV{BPmMcTxkL!09i>tmF-`$J_M zUfGC-N+Dc%Q5OElt3i|aAIv**IJY|1Ok(EF#7~8$s5`rfJiP45cCK%PknKxhQAi6M zPhJZXQ(}bZ^i+IwQ3>3@1%`irDg4fsq|>xzq4>cRv>Ck_Wk&ijsgw7F$47O92IF*b zx278I1QwxT`5mTW?SPAPBZWy%)34Zf=3(&K&ER0IKo5K^1-p+f7;(c0?)#TmEOa`7 zA)&djCpeP*NDs#f1u1%VtUCS;N<&$n32e8j2Jas%19_(!K>d$8jtwozBlZ{g*l&vXHleK8d@ z6F0ETC+p#7-xSfSYs&Ciyc=yzIRev9t3lweDAFhPSLM8n7cDlBp@YY(LwTPDF5N}M z|KL8)5LEqnLiE5MB4T5L@eJ_-aYuX~kMpa8Cx~ z-6oh{zYdMdtVQvAmcicVgXrKY1(D~!ciHdOm+tM5jrr5ctzR5(Wwj_ zO|nFRKL5dn@us5LM}M>NSEBKS)l_h6R0NsM2r_f>TKrHx9|9IW7Dm|ylbbp|tZV)u zY->EgE@@oEiOR3YhNB-@g_fm-Wc(a9?KR+otQc4yA1D+z4}*z=gNgo(E3ENh_uQ-B zu8AhDi)Nu}zpi}vIh0jD*+b^}%E06+{cMoP4#1X@chdinAptQkk9e|}%il2Vs6PE| zr3Lm^=TL2CjmvJ`LZfPFaGSk~70qZT1GSATs;Efl@7slo^`FD&AyT;1S_kCUe`k9_ z!f}pW3$Zx+js;hnF~zJ+I6`tYXmJ5EmUfcpieRYqk)k)1&XO&TTST*KClJdiZ$%5w zpT(i!ad3acDY&U+imnM3ICZ8euGBaWD!$EZe&9Y-n0c8D^`FW9x#C9j6(#uD>U{Fd zBN9z=*AnH{Blv3lPNLWD4V_a*;fXil7+b1=Hw(R3eRm72PF;Z~#{A3L$`sfa9c`>W z`h}FvpAH(Ye-homTyje4BCK~&K-tJ1HgxrBP#H+VnrtIH?K~5#<#&ST@kmx;DrS@y zhN7eVR^j32#Ui=h`S^2G7Dky?z|hYo=%?Y2qOs@MbKf%I#p`5p;M!Jju|13@&9!NI z-(I})zKxBp(Zo@wROw0?BT?G>PsGhL2P@1Yabn*>u=u%A6nEzU@eNBB&plI^V$5*d zE2jjF_5EaJdq2!^H^c+Ol;{Y*w@k?_icWlS4{Kkzf!dxd*f`RI+5VkCZhm`0f)u1` zm9CxxC5aTWIQcY|UeLhN@yS@dybH_J{*c?{b*!w{04qvwVh1T=Gxh|)sE+aQ_gD!z zEj1c+*c6O&l^5OGGZQGECidV=10}qXpFjnzLvV zFVc3v8}}_lv9O->*`V4_$f7;1W82@ zm5rUa&%+xdo6ONIKm)DjUuFBRP9yZZ1E`uB(p!?g7{6^KCRA@kX+tye*Lt;}UVE6? zA1Fc9KcjHQ?Oo8)Hy0jB zD-!tE=veZjI1rn=Ucs@#eiHr0kdIR3;Qg>3k4+mvP4&MM_Cph0zODp~iV|EQH3R>x z)rR?-f*`4NGySveIc{68$p_idx}O!BASpzH>niVsSpjKeLs~P0%cVi%39*-E-wFH{ zT8j?hGIaiNCAjioE=?#zo5? z;^laEyepnN6HTn*N`<^VVcf=NCe2FK#GN^V=;Ed*BL6v$56em5w<81=I$0bcQ<507uaKXhWpU>!Hvml#lNS;2%UA$q+(%!6(-=$O?$i|&i(8fyFa`AcJIe_J!U_nzksdZ{?cHH~!bFNKop zzC4C6q`PjK;I1`ccyRDuY-KT^JJExl){dvZ-&0WvB44U-~@7K8)R{b0kO30RFBEq)8XI(C$S+ z@NJkZyWiu-*M6->9_uE2b{$1wwJpc+?bNI3DEYjj2SVmu#QDWTpyA*;Zne(|i_Da1 zw$lNa-XKHI9WG*KlBM+Sp3j(O{|8+Ut)Y8!N6^LM-e$nxMpoPsCb;Qzuxmq6KwTJ0%EzJ|D+(T{!;D+6i5grFcPvK5bti3)Yv~L0a!RX*e82r_1W{f+r+a~CV+h5d5KwSTgl}##G`q+RTYAH=#Wn)}+|}gn5q@xK&nI|paa9=C SCig$H2oFyW=LBbGqyGoMX5za5 literal 0 HcmV?d00001 diff --git a/behaviors/custom/Fall/Fall.py b/behaviors/custom/Fall/Fall.py new file mode 100644 index 0000000..398e719 --- /dev/null +++ b/behaviors/custom/Fall/Fall.py @@ -0,0 +1,43 @@ +from agent.Base_Agent import Base_Agent +from math_ops.Math_Ops import Math_Ops as M +from math_ops.Neural_Network import run_mlp +import pickle, numpy as np + +class Fall(): + + def __init__(self, base_agent : Base_Agent) -> None: + self.world = base_agent.world + self.description = "Fall example" + self.auto_head = False + + with open(M.get_active_directory("/behaviors/custom/Fall/fall.pkl"), 'rb') as f: + self.model = pickle.load(f) + + self.action_size = len(self.model[-1][0]) # extracted from size of Neural Network's last layer bias + self.obs = np.zeros(self.action_size+1, np.float32) + + self.controllable_joints = min(self.world.robot.no_of_joints, self.action_size) # compatibility between different robot types + + def observe(self): + r = self.world.robot + + for i in range(self.action_size): + self.obs[i] = r.joints_position[i] / 100 # naive scale normalization + + self.obs[self.action_size] = r.cheat_abs_pos[2] # head.z (alternative: r.loc_head_z) + + def execute(self,reset) -> bool: + self.observe() + action = run_mlp(self.obs, self.model) + + self.world.robot.set_joints_target_position_direct( # commit actions: + slice(self.controllable_joints), # act on trained joints + action*10, # scale actions up to motivate early exploration + harmonize=False # there is no point in harmonizing actions if the targets change at every step + ) + + return self.world.robot.loc_head_z < 0.15 # finished when head height < 0.15 m + + def is_ready(self) -> any: + ''' Returns True if this behavior is ready to start/continue under current game/robot conditions ''' + return True diff --git a/behaviors/custom/Fall/fall.pkl b/behaviors/custom/Fall/fall.pkl new file mode 100644 index 0000000000000000000000000000000000000000..d7976e48b0a8c7e2a211a77fee4d0a7835de3841 GIT binary patch literal 28854 zcmZ^~c{C7T_&;vTk|oL>MVpeOh%on=7SSdtNh(nhm6SJ0S}fW3eJ5K=o1K{ZOel&- zluFu^7L_(>&)4Vk{hi-mzu%lWbLPyk`j}m#{KVuNl{`4cK_c; z5|h>l1Sc&Q+?f=bv>|Es8o@P7f|FdwMUC6-<$KWif0_un7$k*<{olL7QG!g;x^?ST zkB)8sdsrjjm9#T_nSh{xK!9x;eRM2_XzU(G3WfC;Ew==^&`ATGf3?z`IvTXcG?M#V z*via!;6oSR|9RzxOcdcvWr);cDX`Bj=bDKvvD%`9$!9z$(zlW7+`n#S>b)bRDM1u5QV5z(^w2i;1Lq`gnhM9vz^6Zd z6XEKM5;xNBK!0Qx772M>8$NnXzsdf?xfEoR4#o?lE!+% zRm4^7e>&7v;{WZ=HA*2#u5x2tq$D8V_kdyN*ihWmITK3{YVv1?m0;2e2dsLx17At` zqg%moFtIv??_SPkPu9ruCN1*pclH^~H{HWoe0G92mHVm2t&6Zz!-%_lPgyqnGh4 zSeDa<;~vz(WyMtH1Eio+rvU5t@h}wbod8~jXQ{sgg&`Z zuCX3gUyFh6kuGSKo)6QkrI6vg*vBH9Svz(czQ{HsDcm@|MZ2A-Tvj%XUzi5{y4zsM zf3xt5#%)k^%%jWG>q+2{2L%1F2C<}AnBv}pO3{7hxP`Cq>tM>A3vu=*Gq}^YmFgP#;+6{)Fxr%l;%Dn{#3~dLyNvkK1mKG&)z#m9 zb_OL=MDhDyGf>)DgfD}-V4L4OOywN-glv8AmYD(}M~tvRnIpGK!%63&ne>-#8{U;j zt$$eS1Lu|qWB+$O_zP8ZNW=inUGKzZ%LXQuH-PMB8)j*K4Xh4W0ypn3B1&r0sOLg? zuyfMn-@YEiS>vOja3qdQSX_nwI^Gf`=XtzE>L`9XwU;E-#nY$h5@7XnBEQYo98NhZ z!AG;rctlyB)o|5y-Y1F zdvUpl3Av#jg2|6HneO(*F!@&!J(8Bhoe*he9ONDO^JR|MA+{2-M?Vl|^KUw5dJyf4 zOCbA#=0ncaE|6ysA68RryoHGWE`c7US|-?AJN?2S&?x6%v+MY?+Cea zqn1jJjca(|3_SJy4h{Gj2oH102xBeC@6IxX_+dGE)y@GklBUo#YHpwuTSg?dNboa8 zREc}YYTS?`1mAwfkS5pppjNKKHcIA#hHWOaTU0@!Xg7JI8cf9lHE>}0Uekv^Mfg=J z8X$9~v;IT%3RYlMA`e9dbj#s_(-Wc-3gyL(8j{XcKk|i?QuS z#;mVS6t21x3h7g%@Mrfm`c+ziFY8psBjbO7-;6r?`%5zn4Md};Ts9i2EFfmv{&KVa zo6mGVDknqRwi3%$K^VV%2`@4FoSIdf11r02toFUnL@HuBJP*o)cTVTHCgmr z)w~N3*I0`_b7P2ujS!#NX}}k{EZ_>Y_JG>EdNS;&MPlZ!#emb>(fG11@r}O--$w=U zhf69-*Gi+(on8<%_98Mx3;B^B<8Z&$Te!UbJXn?#K;0Zq+K@Sp*Jw?Ho=^q$e2hNm zOnwIeHRE}6as-55afCS{2${?4NvWU`jFWM~q6z8D!N0N~^dXaEwl0K7^)DzmH5$J! zu!6FwI&8Dw625nvIdXXTD!R=q9js%#Ak zVvQ&?tPK^i-B48Z7ng8Xp@zK2_4XU_-KJeIX|oei?O%n7h9Z1vzW}^z9?yH*1!IEI zDX_R1j!m99G*9Xx2=)R_4oe+dpL&U-`BHxFiyj>L7zZEbCE%!LDB$*q{0Ga~L`6&+ zqR#U4?G`cYl8M2TZgo~$WIS&pz8g%F8GiJg5;?h7nclt5gKbJa+}qR$A)94cUAyY%O`9S?;nrIi z7hO!}dmaV7+}pTaVG|tfl4j3WB;vGzn<%L_8wXan!Z(*0c;QP1mhND&>RSg@c_Pa$ z6|Tj_8mhee`vQ8r*&F|?kHx6Q@gOw)3{JRL03G?-$V7yp!RGaNb=w#4T|En|mqcK3 zQ#umqzufe&L40*02a9drLBn!a+G_Kb>w}r>hhH*~@Y5f&%S3o*^@Y&r)eF4VR+RZ= zMb_6!@=C3a?5m6vtdctc12z(DOzT9*jy1+FHIMM%zbj~|84JgUCbGlriBMT7%JcC{ zA+fs^#%(L6$%36EWIv(3k@oPBD)Eoy2kA@cHRN;pSKM`@7jM1JA-m3pVAB^54B2rJ zKPTxBfkO;7Cyb)k=zHeV3q>|Gd^b5Sa~d`L#rY^}GqzvJ6T%9v;1?VRmFKFcfRiCs zT1c?w-t(|6J%zSwq=2D-1RRCiP+|M8-pG`LOd(71CUyWf220g5CxcL?ejgEhe+K1? zjL>+6F}KF-9SB~1iK&_;0CM@%vec4)>lpwUqP;LRae(x%tHf_Y2jE0iAeOB@L{>jK z$Zz=kf!59XO{<1g>8&7B=6d>W?ucIov5Tk%@jJh8QM)ES9JdgU_6>vjNGdkI3Ze0v zAHoTx8eFxM!7CqYutvTZNBWNO)m$(d*v}yMX5^5d5dt>nVv&`aNM7~5#t#(=kiU)K zs#_26=!U8rFW2A6~WqB*=cQ%Jh6@593?2e=H6 zL!h&0H$#^ujvcC*bo-oQ8tM6KJnrGtNH_`VTh)sq9^D{W=G6C8m5@j3O?!HKe7Zru>U{hQ#Hd z4KI8u6^=}}h+9@Vu%pkOW3~iOrT@Jp4@z!hw%|b+*Zcx`sUjHlC_)*Dd}d&4Fn95Z z68y9Hj?x9g=wet5TUI?p;r9~kvp7i{_jfXXNI8}q-0u&Uh2_|?g+2H~t%6o&nW4hn zFEB3rE8{WAmgw%tf*aeb>GdN$v}gV?vgB4P!+D3`Z+aEa9QNqi+1O=LMN$Xv~TQ z7Q-&>iS&`)FGlCODad4vXX93>vmsV(rtiO;GZhU`#m-Znm}hc^)c!XGeP@K>dK*)a zE?2^{o`M*^^Bmj~9wF;(yzxIJ4wMbN@YU%ayyKRE3H|fn<=HH(j=4y)It*$WfiX8{fG4E-^6pz|HA&BL7aEyCIr0sO}jotVbMw(qI^`7bu%lb@k$ybgGlmu z+Yi#CLWi;JVlIg^lZOGVXG9^+1wHl$<7Y29!oIkQtmPdh{n&JzQyh*Wvz>9F@de`6 zR0PCWgq=OC2S>!7qTuCR@?dW${F$2!E*ov}@61$`Nbkpe-7<{rhSLd!eAGem}k(`G&4M%u!47WJ_RlIe~4C)9=&i;h^9Py zi~?H1MEZFwxA3_Ed9Jh?YswjPxL8X5maBo!$wa&mD}j1BJotu(kp;&jXs3lL7rCw< z_pCUIX@MuPn3!Xo=MJ(g^)8&BbPklS_JgwcOq}vPiL>Gu=S%mQ7MTvTN^6DFV|mVl-<6o?t;?62_CvjbHZ)B?O*M44L#;&| zoXfF-e{J`1-fk6sa_xFzV=Tq5mWqds1_@kWmK?f8B3G@gh_B*4lh^ZQd9}@%XuSI< z-Y-wUNs_{F%xMZ%Y82DMF2P_O)&?dKA7Sj{KvPwgDy*E1y+jB<#AeMgFkKGLgIfSqGm~+FF?+1VEFY8%r(g+--SI*3!szQ#IGXf_w<9gwh&)C?g-J390%(TTTsbP zTRc463d@v5`Xs__Md0{0$W$=T@J2~Zvuqa3WK|X0Bou;r5cAt*g36Z@$IlVNm(YwHVx|GKIIkY;~fY+8CLY~hQrYG zR+G$>c#O6J9JW@>VkMu%;yS_4bpMbi{4kQ|58V?3x0oZ~Jdgl2anDGScM>$-iG#^9 z0&Lt$CH9P#7JgEfW__P@qU72lTsiiwoV+ZY^16q4N42r^r6_W0jj$so471-D@}gB8 zSYVIj7&igZr6plXkuWMAL=31Nz&tkz%pGZf{T>c@Kre!7?$PCyY}Me&u>#;98*yiv z;$hz;e`;;2#GX15jEcS7HIcz@DLF4k}*zfU4jKFST%y)|g z$@*Dr{q(1}%rTHW_lP2cJ@-N0;t*=D3MFrc^XUzd!{D-Exv|l2A&AWhhgS>RxR(3} zxTIQ)ciecG6lI6e3DKgcZsP=^&gD>HeFm*<%y@b&i1; zxsy$D@8Qa6NxtCEXZmKO3et_5AZL6ae`3a4)IHnBiC*Jq>_>GrCD)F-WnWG7PFlc8 z@rxw2&K;i=ULvo%l=!ZTL!3`%R(<9{5%$)+UXnT&Vf^X^=(qDC7>9Ji(u-33Xm9}j zI%C3$Jj%tBJ``0?R&pbcNARAY8hhHt0HQq?W4c}h&Ut*3uD32CqlU}C%xgB;lw1v- z_8(}j%6ka$4@ZG(YcX~DHui0N8T_8Yqg{~#F850%wf?s3&&85_{o+y7UD5`>T9c^B z1s)DR4S5BPhXSeyOc#>=0G;nZ?Tw|cP{6DF?+xu z@gKGtXJJYAZF*$sGN|^Nj2&zA*}ZeKFj*=DyL-JUt$YVBWK==JeGPoGwnnvm$z<`| zEb#Dj!cCjTvx?qZ`0r-UXcK$}9PYMJhroIK45L*!&#ww+NT}}xe5}k~)4!5w;PlGO!GKN*VBjLmyaok`qm$TJY z`5{tXMeNe~U)0(}hLOs_ek_<^1mICHON<4ct>N zV@>RN^68BTKi62OuEVpA+z|*zorUvY=gkZ#68UO+-)%mNbdzWWa<874To5 zFCKb03^H~3aNKMUX>B&(XZ{Ev*X-0;hf^7FBKRdd-;jc7$B%%ElMcJ7wH}qeaIimq>YLMy@>|HUPM<%(6%)^Y$B zN;ERMU2~Z^pGGk&&yY{LFidn0xYM>k2{?7*IT~cvf}e~gJL!!e+ahm47u3AQjv^Jn zS4ni+#p|%{YXSVb>QByXcg3Syrm*|E8ki|(?ogXsDy-rSJ6xiY(W^%_VOevA{({UlqAuW(A8zWBhm8hxg@;z5Za zVm_9KrrsUEBFQ9ZJ$Mha4WeQ3w=IC13g{)H6u9&p~0&U3TsOuS9w>i^2Eft@|E&?;Gr4Lf{c z?L8&3Hp?1+InCo0Z2ytYZ_AE7RZ?kJSUIGFQd*=0!@{I=2P+8v313V8%dSgQBK<3n6y;OaqoW~ zCQCb3lMx}78!E_PUhbSkm!;ptyV8VKbRT58q~4JGhbrjQs%vB*;W3eG+(b8@$R`2A z(?~cMPWGD=kSULsz_$Ivbf4=>VwM+3=3TEO@<*@J6}SE|${yMj6Lsm)yTR1)$~I8W z$%2(yUR;iSES)KMgI+lvM>_Mf$qu`lq}C;hS+`aNUsabu`?1fA`?@vsAcAskI8hbOZ_xs;06SgbT1HZ+gpEshCt zOV-MfBxM#X_S|OndPX6y7Y23B^RT=2BGg#L;WAkre(7%oJU)emYL|C(`s|Is#p_Zf zjkRMi$N`(1)JywW9TUUS>i$!9Zun*t|%fO z)eQzHg_Svzc%RLMbn{3pJp1^EHgyG|WBW$<`ui}JU)95jqthTr?HQFCmVrxemVxiP zPMlzpiyF7K)3rjfU=>n?i=FPn)8*Y%&D;hL;Zv?-_C|8h;S7G>`Ir0YW(j&`BAD`b zkgN^ja9@Q6NG|z9cUEqNywo(P3znuvpT!|C&5f?rzDXC)TnNvSR#2IedOAT6F>8lE zUd|n21nX~O+o(U`Unp}kVg&HBf)fmASuJdnUZ+j`6 zYk5jEXG@@>_f1%os6z|#%<8p$W6|t+DScv|kF&4-pyy=d@QC3#_**mH)M0fh^qcv^ zb>{%^pIZsnLZ%U4z2`8|FPpjdE*Jl1c9C0Jhaf~!4sL5)BgXgB@bu;ZqBYP4wwFps zYv)w@*ISdDWp)r^-r5q9km=xtMVG|b z>iRRpGj=m=D|Moe`qvTLxWjl$=^EX-bSbX?b_f49m7rAKR4Tr4E^hJs4CQKzan7O$ zeE)76%I}^-+_wtA`8RW6l3v6Yz!oTL+CQVRd>uSnBJ+e(_WI zsbnsy-&E&MN?2n3zXAH}T@pkddrqefPb3b%e-Wb2#hbxfo_KKX3a>Gv9Gu(PL2aF0|htq}h$f1hQjEcJ*^TkD# zWKNS~FZo4chH(V$)vF-QgX%Ew*$r+!eM@X>kD+_02#ofer8YLwa8qS7%*!g{Yy>qi zaMTVWb!Ss~g+Vg>Vh0%a6@cm8UfM3B&ReJx>>baO+s3juf`&B(gRFgjSrYr8!zDoBW)bt zc9hvYt(eJw9%w4Cb}MaG(gMG`E?A~R>57@wxaO4yIp;SUCONf0eS-*ndwwIw7b;;^ z^CVJot`61z)NzlDZ`1obK5-MxuM_JQTjpi9Cq$n=$O+iwLdK~#L{Yq)-j01mjfCvR z=2|p-&}pEVW#x7L+lrxNEG6;MX+xt&+VCLkGPa+;fRgeHXy3Q1ko6=1eUb#xa;Y$T z!c~nP8gc_hia_|6@kD+{B8^_?2rY%7=&t7*lssyCsTSzu zM7*rIgp5wiC%Q^&aq*8!B=)5URCcU~41d4+pwM89K6(i08Lu%_z6efS(xy}W9us3L zD|F1&hNN3LGz+%F-gn{L-Hm1}1r-OmO0bDUV3kDD2SUfJ5h+F5rCnqm%hjml` zl8F=b(J@aP?qxn?1RU4k!}a6g({D3uRbEfR*eKZKe{$sq2-g;7%bJw><%Vq%DM>ult#dX_;u* zBZ%)`B*D`_R}ipYf(h=bI6I~X$JdBK=fZZJswM`lO9WAztpd$+dF1@g&-g4x3YZ^N z*dl0)|HZt6Ion%FciS}hCzOla!NqjdDMh>?Q;W_W$Dw`CL+)dPw4lkWQ*IZG1>kv9;iE_6tOY%Zj z0oPPshkRenv9-yrUevz<+ih=SbJiX>7a+^ukVzm@|NWx7qu=1Sa~ZHh^E4jF{z47k zih+4dCH-%t7!2*3Q1tUYw0@U?%m4h~T-P^H5A#~O_^TD1F&<{7ExZGtzCFRdPfM_w z-$4pv#=-js*Kt?nHrTINjQo@*kf*B&7Y$G0@oBFxtltsx9wx!{k6$qSq9xcDYI5Hc zUK5Agmf$qhhSN99U~8jeh#wn7nFend`FNV@D0gyK^e-`iIy1SmPU0-{up9?lBJh67 zBa|qJXWF~2L+`c@DtF-ushQje!S<SX+bd#Hupel&`?ioti)p|CLp z_wI3I9)FoidZ%3^eMR#@^U`f*-GgUnG+_qD9$bzJX}1}ANRRz`R*PCb(8s;-gP3jE zjrC%>AUt&@TCbL3M~}^i1$nQ?f}?r3<)kfaTB|We-&m3}3o23J?R@qJdy$!QS`ISx zn_=y*YIyN%3ke-}3?+OX(0vIS;Q!|tq)JP{j^tm&bHac0^84RptxPeVeVqo4hnAre z6AVudR51fG!t64YZ6xYfH)nR|A5l1AhK^5);lY+9xLWm#J8@D5uHJh`^<6H)wP-Wi zZ7RZ_89YrLDo>M8p|QFDrHI7UtN{<*7_8|XM9H{duu`cewKH=lEUiF|?4@XZeLXIE z(n1zr`hn{G24vxBWokS+#2uD<3=Q#y_5IcVaP5a0+D1PzkJX)93#usai3yZYhTdvz&^XG&WcyH1t#*JJ{!;Aq zuL#Q3-f&{)RCdQ$%8B#^_)Of3MO$}6u~QY-Oc%f>S33;WOn|9dAJ8=mqhKKZDbXK3 z0J&36Fv4H08T;1~aJBFzIFH2;%a7~H>`&4}y2KBbC*{M}iT)&UHc!`WR;Bl?K9JJ_ zDtO}BI`CDN!YTWrvAsMWzaOn-YK8)Efr}M%gs*3!XWC$*dOXNwEu&#f@nH-pS`;pw*ds_14P!QAQU zV5Sy5=K^X|VLA$cP5&s3@<~E{)dLt9v6&H$SqZO?jzeQzE%G;!L6_||#Gx*dl8OK_ zTBZ)Yc~-P3G6U>#k1(b3FR@)npB7pLz_ErAkWkcu+;@iD)H+cVb9Kdsaf|S!{}kB& zQ5P#yy&!(yeE2VOCG_;o!j9a_^wH8EMA5*LlfQZwjqmG_h3688Qq(+r`k{n`zK~`M zu6v{DA0Zeyasn&1rE*g<*3cpg3WKqkXzo1&>$8r)#=pvp%FXTgbM`{o@U90$Zl0kT z;Yrj}OP)XCR8FkMY!K$Mvx(>LY>b-g4WCV<;d`hd)yk12|NR>vi(XM;^F)h8m+!%o zVhuFyS`KFQaM%Z0_@ihdA|lTA7F2R9liI%5DW7EgUe6D=*yQ2N$V*W==)s?-}9_te8Bf&R&CK&blg7UKq8+ zq+;x6L!zW?PY$j2!*_*`=oZlmB4fh9oYiC2z=;ZIWtzhHM&^O9s|e~O4wA>ZcSwK7 z4A^SJV0Lym3_OoU@fBYgje0Tk@Q5TPuYM6*mqL<%RsuI14JDadeYxxZ39(0Xm!alH z4>GNL0sVFnVT0TpT>VxUzt1e9TFF1?X!{ec_}B&bJ-r)7&PI?72@7o7oJB>bkO>)(R%i&dnw zLyM{lh0ztA=}hJ3L1NH6iXCmjJQ^M&6JMEtVM{vB8ukLnaKI~1Zgbg#59#5{38?)b zjS=#`i_!Y);d|f#(6ZP88uokXV0|KysT>I%Fl{e z*5m-6TGN4>V~C#rxG|fX1nkP0hM%QZQVSmf-p4CQk)#H5-)AG-zw(hZ47@U(yx=6b zS4=^I(*xT_yx_=IaXv|ZEtY=)rqnSFlK)<%i3Qi7U2QFD8%(1g zziZa#cFv_|BeHP6V;#xen@e26I&ee(M0V>=p1x!^GC6OC#^UJ)8t~Z$+3UOE=uSI) zp|c9?-=tDGpVQ>~)Jh!rw*|+Wti*fb({P%z3J$-E2ieb-uv@SXg1)yvK;le{o3svm zV&tiq>JXP!xC3|BchQ!dljNRnG93;)Op5)7nWIwqXfS6BeiAg`oDR;WDFZ;_EOLm8 zUp=alfAqgocVX^`KfKx!4Ps;e!JG~&ls&1$H9KU{x(!YBdhQkIbY%n!_N*g5H(KyZ z<}md}8{8HUNVn~{grUwu^kS(xyIrXezZu`h7qw!jHAY#l*mer;r%FIK+*3ImW&^hilRcXIL#0;b~(Vm;Zl0xoGdIYI76nA7Q%d$WL^JN zz{rjy?jTX;-)fIAdlMeQwY!zLAzg~~^GqcN)c=u5E4G5Sxij6eCUA^HOGnAjBr^Q? z5!ra`JC*&{Ob*P8r@-n|Yuj z*hdylFejf%+qv;rg@xAsz~l@t<8(WS_kj}TlvgeOuvX;Ni#v$_J_pj9=zwX%&ZhR} z3-H&=4RzN0^B|MEfDLlX;iSqczNcyxm-nRt8>F;gjn+B524!%vWR%7}Bq-x5$j#$! z&_nK9h|>cFoTz%PzAxzjmbFYLDob;?zE#KJ)tyV2^kWf@viC`YP7G@Nh-Q=ycfh6o zW@goXP5xJV49u@>A?HRd*oqSZtWs?bobwvOuEksNgV}V*Yz?8(2XaVh`#iRFq=a7I ze-7`SsK$e~Q^|qKaCA}5gV$qYIg;LwZ8N@;kLCa2!ABAFd+SYVZnurB+jan@tuB$m z$QP*fs0TJXpCpBw-0E#|!^xpzE@<0ZPX~66BQ$;yERE0ui=^><^bQ7A9u&c-83yQ8 z90$s-#Terr2N$$rIoZc71lfzywMi2ARDM6aG0kM=AMnM&PoKCg-AaU?rvh-|J~M0M zd9Y8l#>B{A8eX>?w{k+v$PP15v|0cPuhX%0?{qMw-^k=6N~j#74=2y;;HHairO6Hd z;f;(XbX|KoK#3W8iv*zFD^XrkG92sXCDXnB3D}w?%)a@ZNqs+ffV!F=?p~q52VBgj z(eWlA!^cD8yk*#VM-m#3jpMdU3iFzOhw0+w#iX}q32b!?gH3@)N$t(!RR38r5#8{X zy!Dpib=mjW(Eoxik9dK>eYvoxgr&Ep>A?zj1$cj}oR0p>Li;U4aPxN(D$2{Uml`>m zQV@q4!sFTA;T-VSN%PMpWMcCO?$7w>MlD_UABZRsp{tfmA>G^#;*F} zAD$4LZ4H9EJ>Yy;3x)-Mh9AdzuPQnB9<)QN=ZG13xPcy>;RYfTK;YRQ2D%bBW2!@SgGH^^v zHbxo^!lu80_+T)X?B9KzwAEUZr<=X$o#{&auGTW{{KNvx%?iK`-ZjYh3?S!g1y8q7 z0@AV=qV8HZ@v$DxdK1jF_jqy%EfMwF3XSL+W{jIf$C2k#kCU7;!PLFk7OdQZ$k2>3 zs5(w)W|0E9pJGBr5@TS&bOqF@+knp=2g7AsGwA#11BM;5F|4%?#0Nr|kWN+d@mmi4 zAQZ+-lbsF|-;2TApU4DaXKo(Yu;4G-xpc!I z10|S2w>_-D1!Y4pm_3agPdmtUj#T53d*^|>S_?b8RaorXPD-{Ef&PF1)m^WKp^=d| z`|usmCE;Xka10&)U7F4C;y~%%DmumH0Y>CR;nzve$w89_?5y^K115p^s{9Q)MZKqy z_jW;iY#z9k&4O~lOE}JBBDu2YG>XcYkXe@v$-31S@XHZ>*kqQ6>*flvAzd@!qv{Fp z-F%w+??WmMr@=_MoVKf`(^(xM#uuUu*%ccuKzp*H>SwwnI;;0oq{mf#G$9+@`VE`+IT(l8;kmK$0Ajhao; zfK$hJ<0W%77`<9SB|;{FL0K&1e_g@Z-*`z!9u#o3zg6I_p8$%Qw42xk`!nN{%z>`< zfUqe)VV*`N{KSd)THzZNh^YdRleh3(bv86~N0W#f-ms+H3M3`excGcUa2^V-559Mi z>$A_KlkPW>(aK^lTl56qJW5zep7SCt(^^MK&^@;$7ov4P%)1%0Yne)i@=fzNUGMU-9{4Z#&6Xk2& zzoYn^I9Q%No?SFO1?Q@iVOfY(89g12hqs04k z96e$e05*9-X#C3sW(FUJ)!$q&>fAVd?LL_p9+JdebDHtb%cVGB?>IKmdkN>dH4E39 zgyNrh%OUx24TjyO_$>A>l^L>tSnXU4l{aS94Ykpte=`4JN+q82Jwe1zexVvJ_ZY9E zQ_$Wz25v~V;CH!c>_Wo|c#~C#z4A)<;+i(|;OuI2t2qxz7A1K^3EImFL;1&e=fL{al3C|Tr%N>{Vt-I%VY>Tv>xPI%PI2)@4JoGpwl z+D1%CRvfgYnnA>$KzP*B!~FXhKyHm;wn;fUs9|*hf999aPaB1Bon15tlpm!NH^{S< z?ZHsKLW0GUa**+if}P!IR6Wy45erGu;t%(vaUA^wrrfqS_gBSSHxV9=u)JUdc@iG5@k5DLkgd)`A92c-;tN|WpR(A z5iDM93Fh55NYz{cvNXgW*99k&C6B}DPPuUGw#z`lwD;u4%OtWgDU&?ELSdiVN~(B1 z0p%W#*@L(ztJ2o5EbW$>H<4vlOKZTRns!OZHj!=a&e<@j_kLIfk z!T<6p_^#`U?&p`IhrBLVqv*gC1+5`LGEN}vr;jcrX?5mSGK8tM$AQK%%%*P$_jYO1 zWSvrMS*$u{F?B)1gP~}0S(a7c0?22tR>;_L3%j_jbM4%S&`sSr0~Q%;vk&OE9o+tb`#=o7>ecfwQ#qLHvp}_8v|~ z=6Dvl?3sW@6>9j;FbLAy5k>EavBwrIfCKz=uDdTBwSEecS&N#8LdP*YbnF!KFc;|G@s{HD=Ep6_QY`jYebdoV!H`KmB`5Ea!?~U2i75 zRh$b7lVVB3!!EpRc5MvuXfr?63ed+lg4@Wxem_qwFVzhBNyW~_SOp|QG$nr&a zSUQGW(M*QFkL&RAtLNycycN4f{}Agx-?`R;&y4eja(H*X5*DhY5pSbGQd+zNpXTl% zKix~{j$Lt}kZnPj9|5?w_YR%+^*w#;c>^D0+2Hg^wa9PC!r@~UB;LM@m?zu7%^hRq zX;QaTSagJBHb> zUXywqWw<3D2lYAPW7y^q6}JJ}aAOLTM7okEYsA>o2f8p*$`dYGI$-E|9(`p+*gdbE z!9w9FVU-KO!=VmmEtth^Ff&1`t~vO$?IqXxS&a8^-Ur?v=GUhPSJ2}V_Tr!WA@DkS zKZGBc1-;ASA>1OEzV#VG)k#8VcZ6V-tQsAX$%iu|SLv<;GVCXlC3vf9HFYl0#K_J2 zu(3A8&EJyD}@4)T9sq9wM?dWR|LWihgy+g`6s5~ggH#Y2}$6ejgENmeN zj>x01;s>y=FN2`8tGM;J7#fT|$H~*jW8#MI)S)(soSM*2FBMLKZIVl&=v_AGxX&id zn`ffQ4N+bp@)7y&B|>+#ZlmjU%0Z^j6c(?a&b*#oM>p;i!Gsi5w2V~a43;#5W=tAw z8RNYao*?yq^?|(Tse-Sfm(bqf8H~HoGsao!L-U|M6|}15+U;v#6ofFhdmO*>>?Itp zkV02(IY1PzXrfnL82rmqU}W!k!|aGmOtMgbpq$gpwi~vjd$}?^8Q6wbeT7UW*Y87{ zsT09!`chc8BNv5S{6TZyUK+Ne3U77wLE;ThwEhfzpDP$aqjRh~RM0L!I6e-GYbL zOH;j-da$^`79VGR=30CL=Tor<}zw1FL*$L9MeQ~|Tm|poe7cC~K zj+LLJpuxj;bpu(2kQ6D0ac9pkTdymFoZ5Eg;l4;b;duss)a)gyb2fl$crq9)sfP*w zr>--NtLcyaeRI-8qBJNC8dQd;)L!eP2oWlklqvcZN{Gx<8kA^IN`;gL3P~xQ{ar_r zM23uIN(h-}GUUJSy|4T1-lyl$d9c6Vz1HXRd4G0c4ONdjN2`L(=#PN`3=+_auk${^ z_^vpb!snB0*=nXxe;3acj>3fzmys=t!qz5x&>fXbGmZxc8G(Onq<-bZ?f6$W4 zo*?9G48rJf+(@kaRZoiNLU_&V*_3Zlfqii*&^KNIHH(L`4T45B^UgsO_+s*zv`?6; z8q_Km*7#(7GVH4@2C>>Q>=pC8-Bubxfb_o<$;>; z|MH)YM2n&Q*!f~%KKhhT-6P~sX?7r0-Fd+-haD%)@BujJ_eXrzzDMx=1(Ysw73tp^ z#WwEUft~u(@J@0h{U_W>?-quU@uA(cTAYp+GHxVEZ2>pQAYyvo$>qfo>~kI_nJOUF zpSx~xpYahmUzcKAxMo@~?={WzU(YOd79;QTgU+404LQ>asAY7D`1gfU{^j;zc>Cor zJX{w5!=kRk82SB}a8ppz{wyN9Mjvc5T?38U`Lt7R5SmAdus^SgUwj&o4_?G#L-unS zZ=@wfM=!Bo2KG40!dZ0S&|X}ic^rR>Cs11XJiHZH2Q!PbB;s%J@MM9ISNOrm@Lnd^ zIF^C)X9+}%QVOPtv|78_3%aYa(6S>cEegYVg{WSlh!ZGDX}{d^(QW?kT`c!)25)I#wdiJaM(tK^^APErCE ze4yh$Owzr9;s1=NCfgIao};ui-hnzK(&V;FlipscA-lL7YVJ9R>d6UcbVcw0N?Lid z1DEUVzPR9<)fsrfwTEx;nMAp9KOARioMgr$Ya!=r4UV=_r$K)+_{c9yu)*4ch3>S2 z{PZ7ATX%@qg#97><)3RI^zH;sYnC~?8W_Y4n<>T%T_sp17XwX~qBymTZJg$;qpW-Q zdE8%9!5e%jWagfFsG&WO_Em~8Z&N6{(iM$UKk}sNgJkJq&u;2X!{|VET{e;`*8`R54}& z7}+LZPscl&=CO}G+p>=zp?89PvI?Yxj;&NS>lb8Zw_`+V7Tv6#PiHbxDdFxevV!?I zHL3ys#_S}^z_U!Q{TEw&KvyzOzk}I(7O*QL^+?U48%zxPVS!I68|Z6FA0wu*q;dY- zkfu7wG8f^?-@fEOS_cEo1q~aekcWd zoWV_9$yC&kgG*E0DfIetae-eo+j}V*Rfe5n=jJ?jls%il6<;>PhYLHz-o=qvb<>5K z*YdQWaVIVRybOCM#-hxbTuN6=r18(1@tIvZtW&OZa`^6mVUD|~$lHUpSzmYBEsjTv zODCCK=`*JeFJG*B@4*M!E`)RO2@rT#-ck3X9zF0F1^r|EF=3{;h~rDykd6>0_%C!c z)n25M(8RL7zMz=H9k5b!C5^KEimP^1u^vSiEGaocYbNgoiPo&0)4pjaWvg0RM z|2ZLCux}9F>)C|!y<{+8K^dMb&Z3#66Hx8qTzoJf9BQmuV3DwYrOIZ|74QC{vBigY zoy}9k{rFSVchr)`jE#alSCmLnCghfEZ(>5;MS5o#M+#S@Y1wL_Vr0l6kT_V;3=QFb zwX2e%!b<_W?8GON1*GKbW)@&(53eq6z^;W4aKvbDta+kL`9l}uki0Co7x$M*FE~b8 zfzp!t8BDC!=EWWV(Z{BLFmv>hvB!Cl0`6?w>@-r>vxtBjsthwY~#* z*Lx5cq%dK=IE-pe+w)fQGGV}%7K*T|Cnsema$KWJ_Lts~(a=|nADc;e#kCMWKO2?u z`hnZ%T(NoYRkrj@8V1LI0Q|1$w9_sN-#U+`_d}*or$-ZqS&HiK z+j02tSa>{!k-?`NdL!U+#^-#g@MJzt6b$Cv5Ba$LP9qlYekgcxU&KLgUhyr*O(cgJ zvQTTkl;mV_6gOhEu}JGj6`EbLq{SPCNIsc$@V5&t)1uc+7-h|~&`-l8T_$mu6#k5~ zc1*;_?pmNdR*5V=&gW^6DjnIb3mbmLasf)As3?<&2Zy)gn}<8=73cM!_YhN2)Au+$ zkti)0dD=#u*Zp;PaLhO?uo%s2KbwRdm5h^a zJ<98;TT@3Jvz>iWP zFnVb-o7Oa$aVJhQ)vZzVDybQbzFp*UR^6m4d(YF;C2EothX#n&UQC3JqjB`}n2v<@ zwzBQnrEse77N!r2W}zy1IK1LG)m}^>dA}t7OhYJbv@3_IDPQ2|haOfdlEYB%T{!CX zV2XB5ghyXyA z%wMY&7>zpRepeVfHn;kZmh=V)vrmXiireUJ#k#xTz=j&7TH{ zc6_-7qtw(zrneL%A>Z<_I$B>et}&XGEWYRTDNc+{Vq^QT|7zU2Ef51Sg%&Dq13>H<@aA~e1(XF}?QdPE6(XDz?IerVO4^Dv(0ESFe!Wd2sSwN#9Gu4*qPl!&G?fKo8aiF za4s`-96i%HE9l%=c>GWT8yb;F7Y-M}g>5OM7&wz6_BL>?8|zVRsjT?Z==E5doCHp~ z&RDQ?6n)&jhZJYzF@-xJyz&NXaMuhXEiF$>-F+LZ#-vbFR1r=*=gZc0JSA(@5xla~ z9Gc!-#d++wiem;Hz}y4!d_;>Onq4dAu*jb#ajjT*A&{2)zQ)|2u~@(1HdI&6#EZku z(TwaiIGkCFvm4~;eNa6 z=JYZ)<)cuAv%!FJR}G||J8$^C=W=Po-O1SM<3uad*U~U!c^dw<2OgZUgY)L!xwn=& zC^OpwH^i0WNqHMK!s;?z{w8R%l9Swwakr6%m2%R*SJT+V0{_*bh-#BOG12cD`FjoK zG8)csC$joW=8tJ$0aArDYvoY*0%5EkK z?=-^}8vAjmq>FQ)A3{d{#Bfhc5&V=92E~w=Fbrh=%%YDZ`5=&Inz=#Lm>C1~- zq?>butr`@-s}&wcPhU68x;Gpvle=KL>vN|6$PCk$)?;~X8$6P}4AXWk5_rEddZXQj zEjsF`eRKt#So?y?+#@l_wTh@c6=OyiQkhXIwSVYjFXtb}-R(Mf$S8-jx(w(mYXs%= zlkBCJJjht+kt4n3@3q}wIVL4yz06U#C4UJEGxx)fIo`~3Mg!ZHIg72V{{t^H?!w9k z8GN~Op5XJ3U_-U!@teyN{z7~_>Vlc{cDnwr~5ta5`1CyyLXk+8Ye5X&8{JfEbyR=k9axsGMxiTJKPTEQi zF1uinz-N8FunA+U^)W!0i-N4vF2tu<%l9$zT5%F^rhp=Qx!1hW+puDf0=w;FTvMD6$ty3LY00% zzf2#)YJWriQsN$bu9rbOk0()vTDriJL~$Dy5(}Q!&RUz_iLWW{$Ic5O(0(z5Dk4fy zdq*oa&YVVuLnp9El@GY>>@{{#QG(;{1ZM!@-yRh_RvCj&LSyNWZaKc~UQ5;ohvSBoLM4Y+2s_%4 zL&xrCklW=n_HElSYMxV!aeJc4`l>KHwKda;8dIQhDbZG|V)E8F03T+Rqp9N{>@D5K zuh^j@(m&vi=U&}~XkUM{ds7Boi?5*A`~bnEMA$XWlI}>Fko%HAK5}W4t8fWUtkME} zqaeBWZ8x>HMPtsPg*0g6H@H$5jU&USqS;^t@^N#8Hf^CUYHkDTOFm6x5YO^HR?q%Pubu1gm%#ySD)+%ve`m3>2t|4%HIlu4)P*yx($P2b zJY6wfjEkQM^zjA{=pB)b&!wyIolgX*R6J%?n-9}5jbpemt%H`F7Ez>&1}#d9!|8kV zX=Aqr$*qaSYm58gE!E?=GF3@3WO8Nw_>&FXa-9=SdhIF{6r+iM+K-7JJ@iDCC}(DI zL7C)qHj&M|0$kiakJZ|ykaDjvTWYwB2Ic-{W@~lGReTy>oD}xmenZLSjvrg&SIwP? zNE2_(J&Nz`LST`@etKuI4zD&&;^vJ01wHYLa7%9^|1qQot1s!p?B{|`zGFJunI+Ux zJ)VKN4X2#O#1)`&Lw|g9$PL}bhht6LWGYl?MXB0Okk(dXIXy@C&<(z*aO@oSV`Kq4 zzC9hYmYHzX;!1pygE*~Xq~z%)j&jtOP=~3&*$$nHi=3ZvwU6&{a{pG4ZNzCzv|UFd z6C@OUUKUqAJ%~mRbs^;Bar#f%iW&JHqA%r<6!}D!&l7JZt8F?2{)gE9o|km%#aXQK z+d)ri4pV4EC`*_&j^8GGkj5*_gAFxWG}E#ad*-&0)`=Ka8F`L6PA8$Lemlr+LfSIf z1YhsJk5jAG!Q-pz;bC1Jep{RkZmV8^zgsf3*t`aL&lGX^t7aTx--XxxHSv<64`0x4 zx$qt=fg+#rxWE4os?_+1`uCgB=E65zUH^#nmSka!1>-J09*?&{N8&!ogsJZ_C7s!i zz-8GoVu81?)Fc-8wp6sbxSNt+rjvN3kSWQ|q?)`Y>DXo$N_LlaC_maveO z){@=YT3DYnRJ8Y69U0v^iswHcCe0bQoD@$Eq^oYKk}!eGx}jR_=y_d1;(aiN4z}uu z#&0yE&^|v5+LS>K_7QA%C<+oh@I z;B(aZV@0+*Z*W`fFVjEoWvCo3FdY4NLHFwS%y5G*8HcAc`%BVjAdw>%_kY~B2PeeW z1Y6SlnwVMNR-rh3bI_@q&CwP+wAyWn<(Gay725+iXke;d*CG>uUzSLrtx1gT=81su273>G`8^q=(>fB%X*(@r4 z9@nQ9P4~-FaddYHTT@}g`og|aWR8R`i^t%Zs%o5MuL{Rb9%0v`3Ta8;0_+@L%O=J= zg`sk?^i!&ana9@S40Tt2Rxe{W-Yh2VZxs+3E_f&Zt8rNG|J|f8DFFK@58^3i!inOXw*%k*sg_^!Kqb8B^U&bgI_$=i;a1w zLRW>E^K4fZ=#O#-Il)g=*%B|*Mt=YQv>gwb{(tKUhDb?ewy9!i=MmQ7yVc3}Q#y?u zq{oiN$HS_jFJb?n5o8|kh-S;Ih)Woc@gDwoTuC4IK2v0Q(NoyHam}n&)`AjaMfjpR zll^%(ht1m)&wglap+7&YVeqeG@aV!TSUc!Hn7z82S;noUBflcq`5#wUPJk4r>{P(~ zToOpb_zl~VI~|?(9b~6h=fVp!DFJ!c#KAse(NW)lSsNUKd>J47r`5~tTpSPMFFt_y z#c8-NI7@g={;~6KTG;tpwYWb12zbtHciQOoo2jmk!xc+ZFnyJ<_Sa>=%Pf6T2|5A4 zlY`jD2rqP7wUX0XxEvG5#54T?!fv)uo7nn4&<6Q<>yJYwa-H-X^JTdOD zNZ^*8f5OsMtz}ouCoz$Y8mbRl%$b`#V>1rBGTZT`&~)E~wdny!^-IQ|A^X|yopO?Y zH>cCr-ZD5DmCBk|IO1wE2CmN{*mt`v+`SZc{A<4e=S})2G}sR3KdJY}Zmm9c@8wNa zJ#h=J*BODXd>(1;-pLks>r?RTDeTW^1Jpc!7MCx%jnAwi*{dnXSZ?}2q26FChWwY0 z!N&Et)-y+J14N_m|;3<-5#mejWUmU`m}2?a;yJ8ke3I$_852QD#9fHE+zu zg(uaWk|mRH{nlQX9al+r2G;{b2`xJ+J^a(*4meXl551DAK>qP>u$G-q0Z9+pdR-a1 z_)ow%jic#woY$y14R? zO?<=WWIpvsHeWd7H2-eX4?a>&n+60d>*epky*Dd14An`)+{WS93ODP&V%vV#7uc3n!`jD!AG38eef( zhm&h9ViS$Ng85`?)K3bAyQSJV@7@7E+9i+{zVoGB7vF$FQ$FXgs-0ion}8bQzzi@TJ*W2q*D|RPX*d~)W@+g{1zQU%PsZ^@8;q2r1eD3ZU z17=n9*YT8UFjxNYJD;y-#xhq3*wVo~sAQ?=9~gxaA?mXKUm)G}7BS;{CA^X14%j+; zK1GVJvw_+tdHH?qF#5$2o);}<*Pjn0ImIdPD{zlEq$_|wyf4$?=QdON_PULYot*)N zQ6f6>ww<5*wwt%xS;20Ie=>c;QhNGF15)Q)W2TWc{DvChL*)YM zc)g8T`?SKVR|#x%%`UWk-o-Ly?(wJM&7pCPI&afwk0TbIV)hng*uxWEO}4-_ZT~@d z@IV}Dt^xZO^zg>xo!Ty!~#rMYR;1CU3+Q zRSL{$+ftk)9|ISgj&c>IVVM8zCKUUZ!mF;^P!a6P`_wMy7PO9LvX>7+qtRmOzGKV| z|4oNd%cQeiuV=#j1Q(cLSk6g#BJ~W&fCK&>6qgyuGL#zx3)YyoKkSC_jf8WL*1)AG zdsr~fjGEq9BL6g>x&JYt4-+`FKR*VxRytE+`X;!!vOm|+8^c1>g@~_6o({zBV|MGn zsYgJMmUi?b&+H?R;J6cGg_y_JpB!AwDPk|rrsASCb7186!@TogF}T+Z0NwS)+``%b zGPs+7+x91La%D%^_0cOS;XoQ}m#^c#oVvlnJudKn$Aw_&y>T#OwITQfnZ^w==peS67l?k)k^Hp+q?!EkjGt*nTL3zBBT@b7M{VMZ%^ReXTEXz>sZ**sw~;nUFeLjyAr+8~b4 zQ$g3`1BEJ`*ZhMAx(vi5l(-+P4)o^i74V_rk$Isi{ zTHhvq1KDo#F=R&&TfA6?9T6XgtvStn=!Fgt=`1^o74Ie)l_Wsg*)+c7!9a@&DVRLaN3=Dm%XbuqOJ@Ryk*|V z|JyW)A3JLbHR+1$A1F(s$>bZ%FS?Vzw7!Y$8<@fVoi)ec!?9=+b_wk-PhsxsOkhd4 zAGNAvGs@M2y@HI~b*c-JtqNh8P67=+pw538VFVS~2icYR?eGCkvaR1AFwMk)Oy^r7 zOJ8p(M9Mc}$+Jg%yju+W+qsLG)NbSzsth2z^Euz~*pFMUz7FPHI0s6fQozA1f_J)Z z2g^q7WvlKozUb;XP+MXI1+~fSTl8k;dvq~>HtZqf+_HtwYKd69a|*nidyHv}uV=T< zD6`!Qhw)P@OlV_-IfkFP0^=*zF-hvVFhnfHlFqxnZu2cV%B2y1H!{)xbr7ZJ1U-y;;P19V{X^_;N^`D7Jnv) z9XA+`do%+YP4w{Q=ixdDQmN zgrzh3JL(xvrVGQq!_SBN*rO>5>}+@iT)fzyzkz;#QkhckP8RB_PRgkx2%86j)L~~XE7FSY`_ICu$M$el#r=i$L7_GL zGe-l3IC4OzAx*UvaOF=2U|4`5rd^ang_l*3)YkzM&z)cwdVjLe`#J2CuQ||{R{lX+ zE z3vq3?BA)D!lPDi)VkaC7*yRmk{!K0Ujv@s{eD&X>4g7&MlY%pVwwW-%um^ z;2>bW{SONOBU@Gxn$D6tEbvuEDtj`01oNC*%#NDgU{931;OIG7YSH7UXWaxCep{a+FQ11C zVqH@9e8HaXT?GCI_wv{J4Mlq|KNxHC+cEHVe{vh}m)&S=VRxF;ux@$@m$ukd__~EF z^X=AvxgTrT{9prIId~cObC_@k{%qn*Lg%s=^1<`VjaYroc4+wJf(p6oS&-*kt6a2$u^=8h*B!GLls+*S#;5 z`b=tB%E|*Q?w2pqwcJfxFZg4{ia-`!pGexOW1QUDe)G@ji`kb^+i{n|2UgkU47sd; z-)VJ-TAkJLo^A=~P0^rjqcWi(F@oioJ^<}t8)orvfo8)O8gZVxa&E8I&~^{^RgLLwN>NK6Il=r*=*M74Bq0_ zcX*`~iE`oUIJ8+0|GK*f@4N{9j}WQavvM~?Z*Bvhl!N@Lus0A}7Ys8xMlkn_v*76m zS9h}gyfZKEU_^lNpmkhzM=Q`wTJB}qcZRIwI z@3PsmRzUyHNt|ty2^kb!gP@4hZ25*@ZnR<_+j?dS+2j#0y(abK>Sh@Gg5EX_~L51=}Bh z?jjqE5bmtE_iN#N&<4JBehRC;s|l{X2S8jF#q8euV$35)c(|a01;mZTGp7UDl~E@6 zvAlta|9j7MxJ5wb(}~O{QV;JMs$*}f1Qu>q#~WrL%>Lsb@_(R)CDC4}pS_$L*wS!z z>s<@nbyt=gta8|NEi3%K={|E9s7gy^qXd0pDO1q5<~&j=S^C9CY{y^^?uw-eQ>b|5 z^j5Q(U&(dBpOgWtd~PbgBtVwjJhQmG*(q$_R#oof>d9=h$32j~Wdbpa6d_m3fwtc< z<$FgYFmaG8hF49d{QJj1d5y5oXE(#Mjfq_NQWcu%X-lRve=&UlMa{5F;co|qGOwG< zxCtuT*wchUz5sjqEbkHIHdPH%+zOb5{9e$SAXJ9r?|_jS#x${72U zS2%YbOWwbqb*=v`L?h3#f-S?D<{CfRvfY+lW9D@CvMKd`dCIah>bC*mih(q)@2JZ9&1{EI}J<&G->Q{ zJ=B%j1NAG6FwS8MThv_20;LbINb6X*!Wq-r*V<@O7)bA;)iL9&D&L&Bg4Mi&qL^k$w8;g%&aO?*5Q;0FuS5BpQ&#y5ncR^AXP=Mi58T>5s zZQP%IIb7@mb->3wnk& v;29W!jyBEg=#YI( None: + self.behavior = base_agent.behavior + self.world = base_agent.world + self.description = "Get Up using the most appropriate skills" + self.auto_head = False + self.MIN_HEIGHT = 0.3 # minimum value for the head's height + self.MAX_INCLIN = 50 # maximum torso inclination in degrees + self.STABILITY_THRESHOLD = 4 + + def reset(self): + self.state = 0 + self.gyro_queue = deque(maxlen=self.STABILITY_THRESHOLD) + self.watchdog = 0 # when player has the shaking bug, it is never stable enough to get up + + def execute(self,reset): + + r = self.world.robot + execute_sub_behavior = self.behavior.execute_sub_behavior + + if reset: + self.reset() + + if self.state == 0: # State 0: go to pose "Zero" + + self.watchdog += 1 + self.gyro_queue.append( max(abs(r.gyro)) ) # log last STABILITY_THRESHOLD values + + # advance to next state if behavior is complete & robot is stable + if (execute_sub_behavior("Zero",None) and len(self.gyro_queue) == self.STABILITY_THRESHOLD + and all(g < 10 for g in self.gyro_queue)) or self.watchdog > 100: + + # determine how to get up + if r.acc[0] < -4 and abs(r.acc[1]) < 2 and abs(r.acc[2]) < 3: + execute_sub_behavior("Get_Up_Front", True) # reset behavior + self.state = 1 + elif r.acc[0] > 4 and abs(r.acc[1]) < 2 and abs(r.acc[2]) < 3: + execute_sub_behavior("Get_Up_Back", True) # reset behavior + self.state = 2 + elif r.acc[2] > 8: # fail-safe if vision is not up to date: if pose is 'Zero' and torso is upright, the robot is already up + return True + else: + execute_sub_behavior("Flip", True) # reset behavior + self.state = 3 + + elif self.state == 1: + if execute_sub_behavior("Get_Up_Front", False): + return True + elif self.state == 2: + if execute_sub_behavior("Get_Up_Back", False): + return True + elif self.state == 3: + if execute_sub_behavior("Flip", False): + self.reset() + + return False + + + def is_ready(self): + ''' Returns True if the Get Up behavior is ready (= robot is down) ''' + r = self.world.robot + # check if z < 5 and acc magnitude > 8 and any visual indicator says we fell + return r.acc[2] < 5 and np.dot(r.acc,r.acc) > 64 and (r.loc_head_z < self.MIN_HEIGHT or r.imu_torso_inclination > self.MAX_INCLIN) diff --git a/behaviors/custom/Kick_8/Kick_8.py b/behaviors/custom/Kick_8/Kick_8.py new file mode 100644 index 0000000..9dbfc9d --- /dev/null +++ b/behaviors/custom/Kick_8/Kick_8.py @@ -0,0 +1,75 @@ +from agent.Base_Agent import Base_Agent +from behaviors.custom.Step.Step_Generator import Step_Generator +from math_ops.Math_Ops import Math_Ops as M + + +class Kick_8(): + + def __init__(self, base_agent: Base_Agent) -> None: + self.behavior = base_agent.behavior + self.path_manager = base_agent.path_manager + self.world = base_agent.world + self.description = "Walk to ball and perform a Kick_8" + self.auto_head = True + + r_type = self.world.robot.type + self.bias_dir = [22, 29, 26, 29, 22][self.world.robot.type] + self.ball_x_limits = ((0.19, 0.215), (0.2, 0.22), (0.19, 0.22), (0.2, 0.215), (0.2, 0.215))[r_type] + self.ball_y_limits = ((-0.115, -0.1), (-0.125, -0.095), (-0.12, -0.1), (-0.13, -0.105), (-0.09, -0.06))[r_type] + self.ball_x_center = (self.ball_x_limits[0] + self.ball_x_limits[1]) / 2 + self.ball_y_center = (self.ball_y_limits[0] + self.ball_y_limits[1]) / 2 + + def execute(self, reset, direction, distance, abort=False) -> bool: # You can add more arguments + ''' + Parameters + ---------- + direction : float + kick direction relative to field, in degrees + abort : bool + True to abort. + The method returns True upon successful abortion, which is immediate while the robot is aligning itself. + However, if the abortion is requested during the kick, it is delayed until the kick is completed. + ''' + + w = self.world + r = self.world.robot + b = w.ball_rel_torso_cart_pos + t = w.time_local_ms + gait: Step_Generator = self.behavior.get_custom_behavior_object("Walk").env.step_generator + + if reset: + self.phase = 0 + self.reset_time = t + + if self.phase == 0: + biased_dir = M.normalize_deg(direction + self.bias_dir) # add bias to rectify direction + ang_diff = abs( + M.normalize_deg(biased_dir - r.loc_torso_orientation)) # the reset was learned with loc, not IMU + + next_pos, next_ori, dist_to_final_target = self.path_manager.get_path_to_ball( + x_ori=biased_dir, x_dev=-self.ball_x_center, y_dev=-self.ball_y_center, torso_ori=biased_dir) + + if (w.ball_last_seen > t - w.VISUALSTEP_MS and ang_diff < 5 and # ball is visible & aligned + self.ball_x_limits[0] < b[0] < self.ball_x_limits[1] and # ball is in kick area (x) + self.ball_y_limits[0] < b[1] < self.ball_y_limits[1] and # ball is in kick area (y) + t - w.ball_abs_pos_last_update < 100 and # ball absolute location is recent + dist_to_final_target < 0.03 and # if absolute ball position is updated + not gait.state_is_left_active and gait.state_current_ts == 2 and # walk gait phase is adequate + t - self.reset_time > 500): # to avoid kicking immediately without preparation & stability + + self.phase += 1 + + return self.behavior.execute_sub_behavior("Kick_Motion", True) + else: + dist = max(0.07, dist_to_final_target) + reset_walk = reset and self.behavior.previous_behavior != "Walk" # reset walk if it wasn't the previous behavior + self.behavior.execute_sub_behavior("Walk", reset_walk, next_pos, True, next_ori, True, + dist) # target, is_target_abs, ori, is_ori_abs, distance + return abort # abort only if self.phase == 0 + + else: # define kick parameters and execute + return self.behavior.execute_sub_behavior("Kick_Motion", False) + + def is_ready(self) -> any: # You can add more arguments + ''' Returns True if this behavior is ready to start/continue under current game/robot conditions ''' + return True diff --git a/behaviors/custom/Step/Step.py b/behaviors/custom/Step/Step.py new file mode 100644 index 0000000..04d7f3c --- /dev/null +++ b/behaviors/custom/Step/Step.py @@ -0,0 +1,59 @@ +from agent.Base_Agent import Base_Agent +from behaviors.custom.Step.Step_Generator import Step_Generator +import numpy as np + +class Step(): + + def __init__(self, base_agent : Base_Agent) -> None: + self.world = base_agent.world + self.ik = base_agent.inv_kinematics + self.description = "Step (Skill-Set-Primitive)" + self.auto_head = True + + nao_specs = self.ik.NAO_SPECS + self.leg_length = nao_specs[1] + nao_specs[3] # upper leg height + lower leg height + + feet_y_dev = nao_specs[0] * 1.2 # wider step + sample_time = self.world.robot.STEPTIME + max_ankle_z = nao_specs[5] + + # Initialize step generator with constants + self.step_generator = Step_Generator(feet_y_dev, sample_time, max_ankle_z) + + + def execute(self,reset, ts_per_step=7, z_span=0.03, z_max=0.8): + + lfy,lfz,rfy,rfz = self.step_generator.get_target_positions(reset, ts_per_step, z_span, self.leg_length * z_max) + + #----------------- Apply IK to each leg + Set joint targets + + # Left leg + indices, self.values_l, error_codes = self.ik.leg((0,lfy,lfz), (0,0,0), True, dynamic_pose=False) + for i in error_codes: + print(f"Joint {i} is out of range!" if i!=-1 else "Position is out of reach!") + + self.world.robot.set_joints_target_position_direct(indices, self.values_l) + + # Right leg + indices, self.values_r, error_codes = self.ik.leg((0,rfy,rfz), (0,0,0), False, dynamic_pose=False) + for i in error_codes: + print(f"Joint {i} is out of range!" if i!=-1 else "Position is out of reach!") + + self.world.robot.set_joints_target_position_direct(indices, self.values_r) + + # ----------------- Fixed arms + + indices = [14,16,18,20] + values = np.array([-80,20,90,0]) + self.world.robot.set_joints_target_position_direct(indices,values) + + indices = [15,17,19,21] + values = np.array([-80,20,90,0]) + self.world.robot.set_joints_target_position_direct(indices,values) + + return False + + + def is_ready(self): + ''' Returns True if Step Behavior is ready to start under current game/robot conditions ''' + return True \ No newline at end of file diff --git a/behaviors/custom/Step/Step_Generator.py b/behaviors/custom/Step/Step_Generator.py new file mode 100644 index 0000000..1efd6f8 --- /dev/null +++ b/behaviors/custom/Step/Step_Generator.py @@ -0,0 +1,75 @@ +import math + + +class Step_Generator(): + GRAVITY = 9.81 + Z0 = 0.2 + + def __init__(self, feet_y_dev, sample_time, max_ankle_z) -> None: + self.feet_y_dev = feet_y_dev + self.sample_time = sample_time + self.state_is_left_active = False + self.state_current_ts = 0 + self.switch = False # switch legs + self.external_progress = 0 # non-overlaped progress + self.max_ankle_z = max_ankle_z + + + def get_target_positions(self, reset, ts_per_step, z_span, z_extension): + ''' + Get target positions for each foot + + Returns + ------- + target : `tuple` + (Left leg y, Left leg z, Right leg y, Right leg z) + ''' + + assert type(ts_per_step)==int and ts_per_step > 0, "ts_per_step must be a positive integer!" + + #-------------------------- Advance 1ts + if reset: + self.ts_per_step = ts_per_step # step duration in time steps + self.swing_height = z_span + self.max_leg_extension = z_extension # maximum distance between ankle to center of both hip joints + self.state_current_ts = 0 + self.state_is_left_active = False + self.switch = False + elif self.switch: + self.state_current_ts = 0 + self.state_is_left_active = not self.state_is_left_active # switch leg + self.switch = False + else: + self.state_current_ts += 1 + + #-------------------------- Compute COM.y + W = math.sqrt(self.Z0/self.GRAVITY) + + step_time = self.ts_per_step * self.sample_time + time_delta = self.state_current_ts * self.sample_time + + y0 = self.feet_y_dev # absolute initial y value + y_swing = y0 + y0 * ( math.sinh((step_time - time_delta)/W) + math.sinh(time_delta/W) ) / math.sinh(-step_time/W) + + #-------------------------- Cap maximum extension and swing height + z0 = min(-self.max_leg_extension, self.max_ankle_z) # capped initial z value + zh = min(self.swing_height, self.max_ankle_z - z0) # capped swing height + + #-------------------------- Compute Z Swing + progress = self.state_current_ts / self.ts_per_step + self.external_progress = self.state_current_ts / (self.ts_per_step-1) + active_z_swing = zh * math.sin(math.pi * progress) + + #-------------------------- Accept new parameters after final step + if self.state_current_ts + 1 >= self.ts_per_step: + self.ts_per_step = ts_per_step # step duration in time steps + self.swing_height = z_span + self.max_leg_extension = z_extension # maximum distance between ankle to center of both hip joints + self.switch = True + + #-------------------------- Distinguish active leg + if self.state_is_left_active: + return y0+y_swing, active_z_swing+z0, -y0+y_swing, z0 + else: + return y0-y_swing, z0, -y0-y_swing, active_z_swing+z0 + diff --git a/behaviors/custom/Walk/Env.py b/behaviors/custom/Walk/Env.py new file mode 100644 index 0000000..274764b --- /dev/null +++ b/behaviors/custom/Walk/Env.py @@ -0,0 +1,201 @@ +from agent.Base_Agent import Base_Agent +from behaviors.custom.Step.Step_Generator import Step_Generator +from math_ops.Math_Ops import Math_Ops as M +import math +import numpy as np + + +class Env(): + def __init__(self, base_agent : Base_Agent) -> None: + + self.world = base_agent.world + self.ik = base_agent.inv_kinematics + + # State space + self.obs = np.zeros(63, np.float32) + + # Step behavior defaults + self.STEP_DUR = 8 + self.STEP_Z_SPAN = 0.02 + self.STEP_Z_MAX = 0.70 + + # IK + nao_specs = self.ik.NAO_SPECS + self.leg_length = nao_specs[1] + nao_specs[3] # upper leg height + lower leg height + feet_y_dev = nao_specs[0] * 1.12 # wider step + sample_time = self.world.robot.STEPTIME + max_ankle_z = nao_specs[5] + + self.step_generator = Step_Generator(feet_y_dev, sample_time, max_ankle_z) + self.DEFAULT_ARMS = np.array([-90,-90,8,8,90,90,70,70],np.float32) + + self.walk_rel_orientation = None + self.walk_rel_target = None + self.walk_distance = None + + + def observe(self, init=False): + + r = self.world.robot + + if init: # reset variables + self.act = np.zeros(16, np.float32) # memory variable + self.step_counter = 0 + + # index observation naive normalization + self.obs[0] = min(self.step_counter,15*8) /100 # simple counter: 0,1,2,3... + self.obs[1] = r.loc_head_z *3 # z coordinate (torso) + self.obs[2] = r.loc_head_z_vel /2 # z velocity (torso) + self.obs[3] = r.imu_torso_roll /15 # absolute torso roll in deg + self.obs[4] = r.imu_torso_pitch /15 # absolute torso pitch in deg + self.obs[5:8] = r.gyro /100 # gyroscope + self.obs[8:11] = r.acc /10 # accelerometer + + self.obs[11:17] = r.frp.get('lf', np.zeros(6)) * (10,10,10,0.01,0.01,0.01) # left foot: relative point of origin (p) and force vector (f) -> (px,py,pz,fx,fy,fz)* + self.obs[17:23] = r.frp.get('rf', np.zeros(6)) * (10,10,10,0.01,0.01,0.01) # right foot: relative point of origin (p) and force vector (f) -> (px,py,pz,fx,fy,fz)* + # *if foot is not touching the ground, then (px=0,py=0,pz=0,fx=0,fy=0,fz=0) + + # Joints: Forward kinematics for ankles + feet rotation + arms (pitch + roll) + rel_lankle = self.ik.get_body_part_pos_relative_to_hip("lankle") # ankle position relative to center of both hip joints + rel_rankle = self.ik.get_body_part_pos_relative_to_hip("rankle") # ankle position relative to center of both hip joints + lf = r.head_to_body_part_transform("torso", r.body_parts['lfoot'].transform ) # foot transform relative to torso + rf = r.head_to_body_part_transform("torso", r.body_parts['rfoot'].transform ) # foot transform relative to torso + lf_rot_rel_torso = np.array( [lf.get_roll_deg(), lf.get_pitch_deg(), lf.get_yaw_deg()] ) # foot rotation relative to torso + rf_rot_rel_torso = np.array( [rf.get_roll_deg(), rf.get_pitch_deg(), rf.get_yaw_deg()] ) # foot rotation relative to torso + + # pose + self.obs[23:26] = rel_lankle * (8,8,5) + self.obs[26:29] = rel_rankle * (8,8,5) + self.obs[29:32] = lf_rot_rel_torso / 20 + self.obs[32:35] = rf_rot_rel_torso / 20 + self.obs[35:39] = r.joints_position[14:18] /100 # arms (pitch + roll) + + # velocity + self.obs[39:55] = r.joints_target_last_speed[2:18] # predictions == last action + + ''' + Expected observations for walking state: + Time step R 0 1 2 3 4 5 6 7 0 + Progress 1 0 .14 .28 .43 .57 .71 .86 1 0 + Left leg active T F F F F F F F F T + ''' + + if init: # the walking parameters refer to the last parameters in effect (after a reset, they are pointless) + self.obs[55] = 1 # step progress + self.obs[56] = 1 # 1 if left leg is active + self.obs[57] = 0 # 1 if right leg is active + else: + self.obs[55] = self.step_generator.external_progress # step progress + self.obs[56] = float(self.step_generator.state_is_left_active) # 1 if left leg is active + self.obs[57] = float(not self.step_generator.state_is_left_active) # 1 if right leg is active + + ''' + Create internal target with a smoother variation + ''' + + MAX_LINEAR_DIST = 0.5 + MAX_LINEAR_DIFF = 0.014 # max difference (meters) per step + MAX_ROTATION_DIFF = 1.6 # max difference (degrees) per step + MAX_ROTATION_DIST = 45 + + + if init: + self.internal_rel_orientation = 0 + self.internal_target = np.zeros(2) + + previous_internal_target = np.copy(self.internal_target) + + #---------------------------------------------------------------- compute internal linear target + + rel_raw_target_size = np.linalg.norm(self.walk_rel_target) + + if rel_raw_target_size == 0: + rel_target = self.walk_rel_target + else: + rel_target = self.walk_rel_target / rel_raw_target_size * min(self.walk_distance, MAX_LINEAR_DIST) + + internal_diff = rel_target - self.internal_target + internal_diff_size = np.linalg.norm(internal_diff) + + if internal_diff_size > MAX_LINEAR_DIFF: + self.internal_target += internal_diff * (MAX_LINEAR_DIFF / internal_diff_size) + else: + self.internal_target[:] = rel_target + + #---------------------------------------------------------------- compute internal rotation target + + internal_ori_diff = np.clip( M.normalize_deg( self.walk_rel_orientation - self.internal_rel_orientation ), -MAX_ROTATION_DIFF, MAX_ROTATION_DIFF) + self.internal_rel_orientation = np.clip(M.normalize_deg( self.internal_rel_orientation + internal_ori_diff ), -MAX_ROTATION_DIST, MAX_ROTATION_DIST) + + #----------------------------------------------------------------- observations + + internal_target_vel = self.internal_target - previous_internal_target + + self.obs[58] = self.internal_target[0] / MAX_LINEAR_DIST + self.obs[59] = self.internal_target[1] / MAX_LINEAR_DIST + self.obs[60] = self.internal_rel_orientation / MAX_ROTATION_DIST + self.obs[61] = internal_target_vel[0] / MAX_LINEAR_DIFF + self.obs[62] = internal_target_vel[0] / MAX_LINEAR_DIFF + + return self.obs + + + def execute_ik(self, l_pos, l_rot, r_pos, r_rot): + r = self.world.robot + # Apply IK to each leg + Set joint targets + + # Left leg + indices, self.values_l, error_codes = self.ik.leg(l_pos, l_rot, True, dynamic_pose=False) + + r.set_joints_target_position_direct(indices, self.values_l, harmonize=False) + + # Right leg + indices, self.values_r, error_codes = self.ik.leg(r_pos, r_rot, False, dynamic_pose=False) + + r.set_joints_target_position_direct(indices, self.values_r, harmonize=False) + + + def execute(self, action): + + r = self.world.robot + + # Actions: + # 0,1,2 left ankle pos + # 3,4,5 right ankle pos + # 6,7,8 left foot rotation + # 9,10,11 right foot rotation + # 12,13 left/right arm pitch + # 14,15 left/right arm roll + + internal_dist = np.linalg.norm( self.internal_target ) + action_mult = 1 if internal_dist > 0.2 else (0.7/0.2) * internal_dist + 0.3 + + # exponential moving average + self.act = 0.8 * self.act + 0.2 * action * action_mult * 0.7 + + # execute Step behavior to extract the target positions of each leg (we will override these targets) + lfy,lfz,rfy,rfz = self.step_generator.get_target_positions(self.step_counter == 0, self.STEP_DUR, self.STEP_Z_SPAN, self.leg_length * self.STEP_Z_MAX) + + + # Leg IK + a = self.act + l_ankle_pos = (a[0]*0.02, max(0.01, a[1]*0.02 + lfy), a[2]*0.01 + lfz) # limit y to avoid self collision + r_ankle_pos = (a[3]*0.02, min(a[4]*0.02 + rfy, -0.01), a[5]*0.01 + rfz) # limit y to avoid self collision + l_foot_rot = a[6:9] * (3,3,5) + r_foot_rot = a[9:12] * (3,3,5) + + # Limit leg yaw/pitch + l_foot_rot[2] = max(0,l_foot_rot[2] + 7) + r_foot_rot[2] = min(0,r_foot_rot[2] - 7) + + # Arms actions + arms = np.copy(self.DEFAULT_ARMS) # default arms pose + arm_swing = math.sin(self.step_generator.state_current_ts / self.STEP_DUR * math.pi) * 6 + inv = 1 if self.step_generator.state_is_left_active else -1 + arms[0:4] += a[12:16]*4 + (-arm_swing*inv,arm_swing*inv,0,0) # arms pitch+roll + + # Set target positions + self.execute_ik(l_ankle_pos, l_foot_rot, r_ankle_pos, r_foot_rot) # legs + r.set_joints_target_position_direct( slice(14,22), arms, harmonize=False ) # arms + + self.step_counter += 1 \ No newline at end of file diff --git a/behaviors/custom/Walk/Walk.py b/behaviors/custom/Walk/Walk.py new file mode 100644 index 0000000..01785d2 --- /dev/null +++ b/behaviors/custom/Walk/Walk.py @@ -0,0 +1,83 @@ +from agent.Base_Agent import Base_Agent +from behaviors.custom.Walk.Env import Env +from math_ops.Math_Ops import Math_Ops as M +from math_ops.Neural_Network import run_mlp +import numpy as np +import pickle + +class Walk(): + + def __init__(self, base_agent : Base_Agent) -> None: + self.world = base_agent.world + self.description = "Omnidirectional RL walk" + self.auto_head = True + self.env = Env(base_agent) + self.last_executed = 0 + + with open(M.get_active_directory([ + "/behaviors/custom/Walk/walk_R0.pkl", + "/behaviors/custom/Walk/walk_R1_R3.pkl", + "/behaviors/custom/Walk/walk_R2.pkl", + "/behaviors/custom/Walk/walk_R1_R3.pkl", + "/behaviors/custom/Walk/walk_R4.pkl" + ][self.world.robot.type]), 'rb') as f: + self.model = pickle.load(f) + + + def execute(self, reset, target_2d, is_target_absolute, orientation, is_orientation_absolute, distance): + ''' + Parameters + ---------- + target_2d : array_like + 2D target in absolute or relative coordinates (use is_target_absolute to specify) + is_target_absolute : bool + True if target_2d is in absolute coordinates, False if relative to robot's torso + orientation : float + absolute or relative orientation of torso, in degrees + set to None to go towards the target (is_orientation_absolute is ignored) + is_orientation_absolute : bool + True if orientation is relative to the field, False if relative to the robot's torso + distance : float + distance to final target [0,0.5] (influences walk speed when approaching the final target) + set to None to consider target_2d the final target + ''' + r = self.world.robot + + #------------------------ 0. Override reset (since some behaviors use this as a sub-behavior) + if reset and self.world.time_local_ms - self.last_executed == 20: + reset = False + self.last_executed = self.world.time_local_ms + + #------------------------ 1. Define walk parameters + + if is_target_absolute: # convert to target relative to (head position + torso orientation) + raw_target = target_2d - r.loc_head_position[:2] + self.env.walk_rel_target = M.rotate_2d_vec(raw_target, -r.imu_torso_orientation) + else: + self.env.walk_rel_target = target_2d + + if distance is None: + self.env.walk_distance = np.linalg.norm(self.env.walk_rel_target) + else: + self.env.walk_distance = distance # MAX_LINEAR_DIST = 0.5 + + # Relative orientation values are decreased to avoid overshoot + if orientation is None: + self.env.walk_rel_orientation = M.vector_angle(self.env.walk_rel_target) * 0.3 + elif is_orientation_absolute: + self.env.walk_rel_orientation = M.normalize_deg( orientation - r.imu_torso_orientation ) + else: + self.env.walk_rel_orientation = orientation * 0.3 + + #------------------------ 2. Execute behavior + + obs = self.env.observe(reset) + action = run_mlp(obs, self.model) + self.env.execute(action) + + return False + + + def is_ready(self): + ''' Returns True if Walk Behavior is ready to start under current game/robot conditions ''' + return True \ No newline at end of file diff --git a/behaviors/custom/Walk/walk_R0.pkl b/behaviors/custom/Walk/walk_R0.pkl new file mode 100644 index 0000000000000000000000000000000000000000..ac7ec3eac41b6fce003f7bc25940063b449d7779 GIT binary patch literal 20820 zcmX_nc~lQy_;`hLIX{O*}E^T*t|=gc$r&V4P<6D)etVVThXwlyjGqsKAjo%?s}4lvrV zYwt#*9s9TYZSmf_*E=BPn9SO}8#nCQxzBI!{tbR9CsK}y{cp#VV-h=k{;$i46eppG z6dU37DN!jdDf&*rPHaTVX0c;pK7IkaH~!BfV>X$l#6lRuBDa~9JRQ*757Pqa2QpLcP8iCt}*L|){^9ki)7)R4!Zq}3Q7HT!^-mQ85-Uw z4{}N_bk*Ns(wJpQovsv+C9jpZ+^W~)+h8l_y0x-F+w~iLZ?A~wG!g|gS{J3m5^1dx zq3C-u99W+C!1XD_(A@VUmL6&bf@0~(ctNa&+0v0rQbr@OS#-0+ z{{(ci)c+5s6GWzLmLClf2nngD=)sTI3FNE)9r$4-&CXh%z*orC;Ee24*d`_mE#A6t zV!aNEWf%+APCrEsmcOxFSmMGnQad=^F{$YGZ5^9n4P;%I1g*>pguj=5V7RvgF55Z} z6(3DLoAHtgEsS1?6n9BEj!h_PNF zPG@wUB@=(^z|!Vm`a`;pv@J*>+ICrV_r5=*M^2e85Vs@zMFC0f7NhU%?3uGiTj>Ma ziMaplw}y4+7XiO{D#@4fA%P>p?DV(4Fvr@MO;_H{wb1(SCp$WTb2i6+XF}-L z%X9h8x8Kmtx)=s@#}JQ&M?mvy0zS#T0%sc?*fBf4;L@viVA+a~bj|EVxY4`_KiLg3 zZ;hka#TO3ocg)hbIqFZ~qf0lKe9s2AFNvhwd30PI6S$ruiZ@z6z_=6(qO&&|vjTq5 zYI=&U|2rMrc4|V*txQtlm6M1E;KeHC?zpVtqt)VQNT$4Ha^9VZJ?rrF?| zbPeyc^ud_v4z#kf1$OR9=A18Yfi0V*ux3mY&iLI%v~LyRwP%TZ@O@bxr=H>xPn6(5 zvj-gUc?dQa{&9{AvY52mt7M(|X?mmDhFE8O!_Ctd(FIWoe4pkMD0E-N7Ikf9Pts}B z_n9a&GAWH{b<5N2P1!Wo^ElT3yG!1VuZE{nA~}c67ohmbKB}@=4|9H8BVN~A=`)KE z`s|w|o$|LF-qb&*$B(2ySGPFN`h}3L=BeEE18YgolqqD=A7?gdWD*qXCg6veIjEP_ zBv=|6NG}gIVG_5WAK4MZF6>U{t@j9_zl}HgDHP!e?^>+7F2c*j&!x;j1Y55E4RfX# zb(m)zp3lI)}lUf7vE zgFAYu4?0|=54fav z5kkxoh{w}0puO~*;MoQ@3^+&dzTaZ1IrR_W)<&R(v?o1X^PKQi!Bj7M6Is722R7<{ zW^{7T(@Axo!MOAd)a8b=X_?DO|2rY*4~`-a4s?_0G1k1|LuYo&rW#0@9|g*<>OpAV zLd@|Irc*R7(UTbwcy}ltZ+d(4uQ$H|cA*6CCanfXLM8YO`_|G0s;VF?*axX|XTZh} z?NmA-2c`s{P|= zJ4qeelBxfYFRpv&OqRUggCQ>0txVYfT%Q$*jyG%R@5wb7JZUYVe`|8H?)X?}v8V-7Qkc*$n@zQ@axUIe&&c1WS`9X`>*l!#*>7Hc+;1uoOcaa+W zI7DQ^?KrQsOW6&ukuNwP&N`&LqO?E_CKw%r=>q}84*PAAWb- zAwwHu;9|*4)-xcTSepWlle|WM6<6Y$>R9frrdaF*NUl!`nwGJQZi&l)qX~iq)z1wZ{X7Ya|&SN#XKlGe=+Axge z{x>jz(FK`vKlCYF#jvgiIid#N1P3BhM1p|0ByE(s$Ha@wHL+6?k>uH26G(KI;S5bEzlVC?zJ zoNQk-N(QK+X=@}CAZElCS8yb4$4j`~Qx5+$wfJLqQmTEGh)dVotvIn~oYNg7WJ!-~a| zG2+}?)GtcJri>7n?PP*+o+pUV`xbJ&K^o)k->1)-NMv7ltr02iO zkO?V?q%Z3s*WZ1VaetsiN|qH7ui{sXm~ty)7M~|Dl{i8k7w6D8kIh7<@CW0sHw(uu z_`&+5zr*sPt!%}Qas1(GOMF$P!^yqZr;Wy$w7B35Y1I(r)tk%t(nKTn@5>pyn)wU( zydsFRAM+pO-Phr1<&Q-2%S-rewGP{iQ~8W-P3Werj;%2PY+I8F{PHS?{()1_{(20% z>sJi!F)E-1n!3F2of+7EX$Dt3y$u)huTa0UMc^bW209N0!DC+zsmT|xh2HORvTPV? z{IO+*r;4I*lohTPO(J$5H0kU=&E)d>@5KGh9IUMVNngCQ!<`FlNtdh&^rRWnx9^JR zynJ#;>x}F4duN%vn zCNxv8N1y2NqoMR{ZUr^SY~_r0tbo9QY_OiGPAqlI*@sb%aJbhI#}#hFv*II|WU_&u zyeS^zW?F*sxmIH6(E|@|hjTumcd5kX&zP0J9b7nqYn4)Pr&>F$zk>3Gvha|p zC+;(|$aVi$WYzQpFkHk_N1lb{YrSx(lQn;RSqMa)NHmn;hVsMnugoPuWGwO~PD3t2O7BW!42 z3ewBYl5eLIQNiFZwm&qXbHwWD-K0kxS5^+yG0FAaQ#2uHgDG5kwT*t^v>DD_~U$-s16(bITiP4-C~KG7x{YDh-mG6Ms7VkNr#Te@ujA2_-JP%tiR;L z-(NbB%^SFi>#JW=-HseAflXlkB8Vi+u_ek+yaAJCV8QC`2Ls;p5RN{;tJND-w z8a7En*y=2@%$;q>>@Owf)zs*6g&U}5A~T8?I>65SA=#UImV`a)zy}vcQP5{s;u{%? z!-5ywzqj6aRMcB=>c%IGh%#h1|5wYrHGGEG^_?&|H3F56oPg4~{y1T(0zAI`2^Xx2 zfD{4vrmeQ5VfVy*wkbFtkKc1g&C9-|f1Ec=+?K)rbD6mB3BnDBVEP0l{>XO~Hq%5EbkaUkgSTzW z-x*)Q)yA1dEzuxNW4?mBpc=nVOM&CeTE6L8GuN>;4>w#nMH@xBxj?sLTx9t)V5Aod zJneVTeI;5D?i&C`r>YxftQbpYPD~`~{3*)csiw*Ec97wRQ|b04dT8|LC~lm004@)z zLdfHTuzImDZ#2D)=2*NSCpX-y&+a(G+vn);*V%mL``;T4eO5c5aMcYmb4w2I(7k}a zbz6zQnKX+fcN^)E78~$O65<6}z7Q}c9(O9};Ei%4mTc;Twf{|H$8;~{e_uWh4j;_; z;jQODd3z}Q+B}}Gzb}Dw^?bg;Vkhj^)yC*3Rh*}I0XGJm;I>A+q*Fq^F@>8lP&)q( zEj}rL-p^?$#d`2X7RTvtw|3g_sJdRTS{PK^DhcU$LUm%Flj$>!IOm@6_$FP08?SSU zQQvcll-h3=*0A*qKgK>h%f zZI{r@^e0wzd_dzlx_s1q1s=xOz{1i($Wxrb-)!#({^AMtWaA(i%;3Q9+#H;vD-S`l zGsxw|K(94FMMqnG5?I@YhJj|d+smn;%EFHQc=i*_@vg>A+rAKeGfVngKMcEyR?+S) z>DVw~8LZfLiCOqC$Uz*pt?5-@2%H`LW_@ZS@j1@XG9r?_W>@p%hc@r%9G@t#U z_6RxQ1kic*3j*t&lYY(tzut9c8JlL(b&C4F+MN(|)}AE&=B5HK1{_H>vjA zFy8LAB+hz2nK-u3V0}{W!}|TV*>UYj=yj!>ZtM!?Wwe?aEK?Nl=-?(eRX&gQhs)tk z>lM7-{g2dbcN?@BY@%rnnOtv~1r>jp4ujXd;Fir^a!yf_!s;nlsU;0}7RF#e=`goc zb_xxf{fKzp6o-R{ov7~14@5^rlpfe5APdbr(V;j19~nHudgV6Ese24g5R3YSdx(pW z0$Zf~mNYI}%833w%pL2TMLZg2ut}SgSn;}XbcV4l>D2S!+phcKpdbp3o+RVpzCXmi zdp~^nHw%AGcVeYGL)hClr?8Q-`BYJBCrnurf}3|a;fAD3=(W$A{cV~@P5g|}>7G41 z@>v)g)o(L(rcEd~ElF#(n($w)-}-4}Irz*f!Qs>G)DQbQ4(Xz6SOS-y^d^&v0@@4(#9pV_Y)l z0UN3rkFH7$u&7J`dEO>4J9!>kZ2Ev)rjdgAU9(XAtTFp=mMV1muE14B5`5o-Z=|QP z0_vC7Vct<`HoB~oIA|Uxx3VYj4kGn5rlgn*ghpV1rvstW2kFngg>3eulf34CYr!=w z4b+1Z`0>qRy!`oBRM=358XCQ!`&TZ*xj}9;zy2!e^m$2r`bwyDjyHAHE1*lP?vg1_ z{xKIVY;bG(Y}|B6gY0f}z*|{8q&)FAU3%TgvdXWNId{APQp;u8=pKsVYhBnsqG9Y9 z-F3MA%W7h7zlxGcNNR&5;6r{9SkBERlFF_4$o?#(|Ne^Q!$MH3Sc=Ve&(U-0yGe0= zEqF0sfNmk2y~hkTZrX94$j8F4lPJzDJP5lUYJu-GZGP)+G1lPxBGkS2j$d;81w_2g zrmcrK_QtPD3^P;4f=@$ySt#^6Oxki) z;P%q9L`8BHoxFDk9GV^gnMaeT_oj3-?U90k>(+u~VHxQ87ehBg19$J(er~wnC9RY0 z;OB0=oTl~J{$ALS8=*0ji^$LCh8k2 z;_iM;I=QtL{T~{!2X^a&+4n)j`L}6=upGbcYZ$SRG-Lx5#M!CNh8TJK8@3*u0%6aG zk?dtzzoU=I*#EZRjZ=wCM(YR>+fhZd*aJ-7gJgK!mP#Y9xPbP}0V>&FiFf~X)2mb6 z@w(Z` zG*P-%!Hnh(A8l9~=7jNHJl%0r0j|B&XMgESV#S#k;8_%c_P+Wsc%&I7F0A3V>lCo@ zl9R~Igi(pq%#2A@x3Vr9af-WkDeJd}x2Y4cGva}t&Ma*45YcZFYV2DpEB z2z4ly#Dw1KxIM0obBGNV)M@;sa?5T37~4|E{f>gIQ{*|Bnlid>)<15X{uMIm^c9q` zPNEaq(;!(|jLjMT3V*-v#(7)J*vDc@{PXF1`TDKa_;0Z%YM<|C_~yMddLRef3nX}P z#}0DdPMGbW=Ri;IDT?lVK~`j4!qgou+`^1x>dYd?s@FVvw#h2c(sS(5n46mDzrt zhJqKpXOTiQ4=kixQ-iQJaFE;bwSWq*NW;MoD!lK|5I*zy1Ur7EVvR%yl-#_);dv2M zmpTrY?QQwDpuwj-U&l>6K1{X0NP@L^I_!0@G60*F_9blMuq*kt&J>qoWcEEwSX8539;+!X7jG@;_RdKpYimrm8?$U zK2+%LWajtW#C0-q;H4l50hcDT)&(!|3^JqYu2NQU;)poVRsqtjpv{+bY)=6_hb3tiZ z9=E$Pg8oP`#HCsqFr#uSU7zxY?iVb?73~#p-{UsaB&>!0o9U3ds+P1Ee<7tsmCRs zN5pF4f8efhy5Uv44!dL117x&qG0XfUAE#qZ*BP%w>6=|_;v+(XczK-h`X>?E5)Q$o z%kZp!Jzj0MVHMQE=+z(f^ePul|GWQ#JNrJ;%5nB_488Ua_1H-bfqLVyX2wZ8QK({N zvuAnz@*^=^f>;(be9tFqa^FMQ-Da|*CXH%ochOnL(0)r1 z?~0{@VQ3)v=ReZGX~&bn(tZJVCY8QEyn;Au>S3wKNn~=b&?g%K#U4Cj`o-=t|JI0O z0(X~c34G`P(O~T@RoG{lDQM(9LNvw8Nb{U}ob9fQq022;&TA8V-RK3E)ML@YuWVHB zlVg8{7BM!e(cqdmmiL_J#ARHQV5^pgu#4|p!fR{2`C{P~lsr}i3117i#3eOkk=$uU zmOD#=jup|00tOs&J~1l!8_8AAv7~HVFp}KWg501uuvmSbe&60sX00qDL5agus?me{ z)Y#3~rF&5g{U}oR`8Mnk%%ZFPcX47Kxx7d*!0mwx?3}aDNUwe$iLSXz1{aBghx{;9 z=kMhUZ1=Gra=+mY6u`LHV9Y2?1ks6au~W*W{{Af;JYE$GE91;jbmLEm%O1hij{Q`> z?+otf)8MI!Fgrm?gr6xk3B8ZK#YwyiKHRK=RfDJK*B_g>o8D2d!D|MNU1r2N9#Vm6 z4J%-3bTAA%_;PPogwn}NHL-^Zp^%opL^QYXOT z+DqUwzm$Ix{gW|>ddFCJt)$B=6R4-RGSIoj?Bcn**jKWSY+Ygiuk!N;5y>9I7Vdrw zW;U1T-%=_5U3e!3#2-c`ECYDqJJjM#7Zn-K#Fr-1NbLR+F!esku5bxsB`Oo(#-X?H z-t#4V=7Y(VNB=PJ&MB<39U%n=|Kju=>v%r$9&l&;cv%T~_P+N!s##HI6?`#(Oh2jx zKjXPNawO(qNYOWcHfsw>nI0Y zQxQg%A1cO^+&!3QKZV|p+D4=D_7J-<%W=bnVBUPrUdz;nnGkh2AG}o$aV74Aluw() zn;$ukdXLj!UYHS^UnR#jG3SY6-Bj9LyN+G29Sa?E?Lf6h3{NI{vid!Lur2BX%@|DN z54hhGIQ6e)qsuRljL0Df{TNA1o{7_6`vwF}x|(d2(@C0a|D6n8|Bmdva$0Am&PRQ6 z1^0nRNY@0wm9wQ#@H-j&JKj+>*K)3HFpJ!i6M=A#E_fD~g*W~*(2BcbasI+XwBkl^ z!=z^#aK5LLd48svwwoOxR_~8f=4BC9Cm*6$3YWqBy|?Jgm=dr)EX0y4f64JL<(!Sz z7Oa2$8XX^rW5ncJm{XpQ55MIL^xMO^agTTLC%PZA?w7{0PXiy39RXobwLA=N<&JXG z9pNBh?oU(2i^%mmig>b}L5)j|@V)pDj&olLDc^dC@f&$8Ou0h3hwp*d3V-_b<$2<_ zJr-B_PD70IlKhn}#5nS%;$%Oc|5G=b%>=P0(wOpTwRZIq{2N9$iivUBqD=tGlm zSUi}3Hn4?xGjSd15jjndrG}B4zg?klS2*lyTZPRjIWX^{3{-a9#LvGb!23f3gvTN1 zY;ear8fW-BWrpAqZUrF|qcO!;&~V9h0>20?bzaP5w^E;f6{c z(J_;Xm<)@H7!ZsQ9XN>;jj|oblxh=H=indP}Z~ zaem;!-zY!Nwr(@yuic7)|E_D(*ex^hjC?1=OO7RVe+8&Z-9STHowe8)0hi+b63L%0 z>BG}Gd|RCv#y_0OD>u4>qGdd}pq`GhqWjqQ_P;P)EeXRke8{g~grm!Rm^Y`|I7gUB z$9=tz^}Y9~t(y>^yH$#v7&RV#2Zl75aS6;^)oG9^N z>iwRC)a<2Joe9`G%1eeOej=SMmCTs%NOED#I{xp$+5EVFzZqXH1AfeU4^aW{;6`*b zh)y2B-`g5!>1kc)v8rKZ_msoo!+Au}@C^AJAj{_b+=O?twnO{ZB;@4&W3YUXXh>Lq z)AAJh>tO^Jt#^uh!@81ZZiCFkvGb5glE$oyfLnLA(nJj@xarFR=B?x^;$`U5{iPu4 z-Hm0|QIPHvM`ZLSqfXsEIyLhXxJp^^Z=71`Kwvv=$T>~l%xYnBZO@ReLkZNG9i%0C z(y;5SC}bS=Ma3R{3KMK`(RLfb#82_eN&D?klP5+UerTiD=6j?5g&QlDoD4Thd|9uk z5pGaj*G~~4ojxg@z%->X^tvL^F-C2jFeLr#026H-7`aCm!+}FGa`!`IyYO;&H7rW6v|Zq0^S*JSQ$$(G56LjAsQ?#`6=p@2ErQqa-rN_P zPu%)k3*2qGoXlFELrhMk5V6@Sxx&$T@yY!>{0MjTE{Y9b$L4_^9Gp-F*I=0(|)zC3RQ?KSoF+Bw}#QZ+&~}y1AJ4UEs6m2s4e3I==VXV)$A*kxDR{Vch@PG$OAQ{VL+Q06Frc#) zsNQQF=a7cwW@)_i>qY!c3u%lBo{bq@viN$DEE4}KV6mj0{}7{%i}iK+rPl+2+1f&7 zRShxZqj6ndpcZaVZsmp!*pZ79r{Su_qndzS8tKm0AX!rS4TEOU;AUeoiZ_p=+)ReH zmZk9j5+aQuJ`+75dOvhB?0psmGk9 zoP+vw##7IW6PaF1w!L}HxE*X^-i0QS3s2ti1&%^&*R~y`vAcrpzxj&}URP)07Dv$5 zJ?_l-jK2+6T3hk#y!B8qb`3rey^oMxL@iq-c<1c+i3}p z2J;}x@B|696$Oc199416A*y6L22Kta%(E)NAx9CCq(Zna`_jSdvLgF>PAk1B9f32A zo{*0>#Ia<10UlJF$*&)AV84!YKvUU`*qUX5>OBka&!j2*-mX_fv?-e<2^##k{*#cT zBEp-TozE!zX@I7@b)bKJF&k=h80=3<5W5?`eEOfsyvp4M6j96M0`Bi4(?2J})5fzf z5Wf-4b1N{Y?H4q5HNx_h36NMegFiiT4maAHL%dZm?0oeYF3J5RLZ3H-qT+2L)EEkD zmc~QW(`bls-;Il37Liw((_l^BaBSR(y5QQ=BcZW>3Y~l9NQAJ-EUF)MZ9J zx510_bgvoPVly8zVyCdDwt1qMlq%n!mQQ@D*Yi8#73ht3tUwcJ~=M)Ji^z zQm!U&K&cU%|6QjaB&<>1K#y2?Z@`gcGc0-daMVN3;|k-}z(0wTtYMxXdp{rP4c#!- zm@mME+aVW+w*6jiJE_wdQ*<y+3r&0X|# zCPRqiBF3p%8fAO0(NnvV$n6WhC_15oNIW}&u6iN$Z6)@c&-+H&p`1j|`ieu@Twzq1 z;XseZB$7W%-Y`A$)8J_4C_c<9!?!=I`J;B>SdtVEZi^XK)#eo3?<=HobIxP7UMyP6 z6mToN${IW#tisAMgT!L-Q9M<6naN)xj~{ZgaJ#`S5}*H?wD;d;N`f?CfB879wrGNa zWl^|GD+Q#+s>1tOeN_8{G926%M;qpUqVCIXlatr1$*h4vJd~(K|Naaj`vZTFq?8!) zVf95)cI^%hO|nCYj!Q&pcMf+hPz*$6dNFxs2NfGuuXKi}&|J*PG^g)A_rr3sgu7=`c!tn^29B5tgXTYJ3N zd0%H^ywn`H@=ppZ@0sET(IpU&7s6{svv6p}HwaNtW^3Yf*)vsY{7tdhddk=jdxCCv& z=K;IJ4Q{;rOU`${AdS20xn-n^I#g=#c27E}VBQweq@T&H^p-)p`QP!WS09Ykd5WtJ z3Gor8Qq*5zh%2r-i60@5Ukn+v?&&ZtTkS(lM)Cf3>#-!tji>obB&buB90ol(0_CYr zVDY>ijPB2(g-dO@E&17~{8l@fqvy$l~qFf5czH5`Sr{!MiVB{IQN8 z=I^7WbWL{)arM3pH}7b%YHMU!xjkjLw0Jx}tIvw-D!S1SnQ|D#^zTwLUIecukAu~7 zE35#oHXZ?DDMonuXWlY1VgeZhdZy80E%E!i6)Ug3uLzETWO8IwL65}g>q zObnu-UA#(tRM&2LWnL%iudB!(vK-GduclL%=C^!| z-6XDGq<|*P9D_!|{q$p)8m}>RfTSsO5cS6;pi%e>9(;TPheoxG2^J6N06(3{Y!v4I zjOL;3kLbnb2@=T6pU$t@bqq~J^MNhOLYr29R?`0kKK^_ODl4ah`Z^^Gy=a7>Z%(i! zat)kGHiTR6&H22(T-dgEF(#dFfyB&M-p95KE=fOz{F8a4WM&XaYkx&!t~_E_I6AUB z)2G2kQxR^}tq0h-VhYsNAnyLB1ADf7pqI-l__+QPbp}Plu&qlNx3*j%%Y~{KS*b7d zQ9_f0UwH9?2d6n@TyUfQKJFxPYmUpV&^{Ca0X zb~NaNj!HN2oH<0l1*oIMZH{F4H*xIeCtQ8$9B^!_XP6KBiSV~6Wcu|lw4zRmM1)GP z(~ca)Yu0LbP*H(|ym5i9o<45AqaHEtt!mJA4r*}h6T%nX?zr~G8}KPR4)RhF?452E zzM4Mff)6G#eC4y7BYKl>pYk#m;@d_(Y>v<@HtJzuSUt9Z}qCy*5Z2r_67>ZqIU# z!z9nMjQpu?BQsa80jbaV^wCxsl3t>UpS{QO(oA#xFX?e?sK;d7BsLpoqa!S|h=9$H zJh3Imf!{)}fb9wwy53cYUEkn~z)C4v6 zSMetoW}@=@!z9PEiyXXNfc(K_@J7yrR9hIpmZXXF$Hc{iGc;g2R8L~#D|@0~Uj+x1 z3TfQ2Mp_kcih6%9BQ`hukSTZo0X{#Ntiw_8vBQME6IDhl[cYA(CYxsWaGjKmn* z8!+;=8uFSBaAPk_rnN`np?J+~u=GoU5i?VG8`%Lf?20+_KvUkP72BCE4`dgAqm~ zYBD>;Q$WACUVwL*mF9a&y5%c>gVjTya*! zSMgdP_wy>Lof}QwJn{xBrvTbM`zld3iDl{@gmG)VLuvB+_cYLuA<@O60*BA(bj!@g zL^zP8<$crHNl%u*^IA9bj-7+*)-LQ%r9sxjbrJ?@t-ynultHt?6W-Sx$9IjiYTDseG=R+A40Dt)Nlk`j{EL#dEjFLQUI1pZEo!j928iI-P(aYc0^ z4Wm=|9A#5}i+w((X;(v2VHteA=tusz*U(6hcOcllm$b18xT#`03?E#;hF0`pcVP-z z{s?5(nENm`>YC^&oJ|*9C?Sal1W;5hN{^_`C398-XYCw8x|fY7@1;`cZ-r|#x7L*U z)g58JO}w^cRJu;MbcuTY`g9)7fEd=Po(??kc-{b=l4 zS+uKgrLop+*q(Th+h`t6-p|uycQYrLGn;BGcW17KRfiz)Y4F6tk$ybz z0tdsL(C@$9@NI7_Iv%ek19CG+z|Pg6@j;o^O7Eh%ORdN(-zw1RTn1$w&cyRy4gKKv zj2SfY!}-k&Z*f_UebM$4?u_Pt)$UwQ2Kw^p^nq{OKCfZ=^}0M`Zj*{*>Rl~UnDB`49p&XAUMi&I zM<%IRBL(b&0_LnAke}u%wC8X-m2hiho*Tr{3%%Y%;P{rgapwr;ImyhQAjaX2bF%C= z=@vF!;Wh*xF<^=~cVb!{Lf0+{C)BDJ_lJo=)X{qW@Es21zEq%Hk1c$^Ez}@3ZY|b6 zeN052R-qth9@&JBwp$5;*ScPL;sGl!2q zyd86$qp0AWOf^E*N1G^{bX z)B%jD4Pe~3GRT>^9KAo!q+7!wX4?<4@bwHqeX<)=D4uL68r}w9eh(6v zJwmWnlwf7nB0mEj&C~;>h|6!v! z`#gCPz3Q&akNva{#f2@&9zb-sBEfgHY=jxf`mAo<7g8DAh>t?YvbSH0M z>Ifk1iY>@=m*IbpEWtQt3t3h->XE*!#bf&ivD5V;appAfy`>j*{d1mF#GWL-ZB-bw0p?Z3*BumIK4(Ct&5=B}kNHg2CkN~l+H<2BDw+pAhoB`LyNKbi~*+vH$RZx0^2ZGxGGx0s!uqsf5EF+MuC z2_(-};4QIIzU#q3^x;Lg6!Upx2@_7mDnrrt@hERscV!F%gm6ivEV}j_!3A~&Q1<&J z9BmI{mRJr_+kH3jR^en!cTT3!aqob+(1+&+UGV22bzU#!GW8s*Ly}Jmh@YDytT^`= zO3#dHRTG6RjqjTiC+|G!M!+9ozX>w+H9){Z2e#>?1g$1UK#e*|Ini|~KD zA~58`FmZNT#eWk$%;?|QiO|pa?z|qKw)_pohzgZol4&x5M=Tvt(Q#1lu z$q9nqi(>4T=2^J9TMCqa*O7OUGT6jHPs+Ru%@H<$0K41*J&LgCZOt6b@nCVX~d4<8Y75Wf7k z2PC$HA|!g!XqiExVim*Q?=!ET_OT7iUY^5}>S;__p@6>h-wONZ*KlipMR7Gu7+PH{ zV5ByEq3PdqXr=5|I5t>=2~tnFqaL#b`|`)bWEbQBOJiV6E(^-*?vcK)v*CA$ zE2r>JhTG+3OH9+w(}X2y*wUa#>@HSVy4dv5yZ4`viVz19m~RUOnj284#+XeLEaH{# zekW(AJ|T)F6Gn4tO0W@Han00tDy#0!{dYu>cNb6Rbxc$-=)*2-e!iSNoGQ<@^(^8a z4ea3^y!Vp$yRX1_%t7$WH0OJ5&+-y=CDiuFQr2RV(dZnkq{kGWk@?G$pk4I<%~@#9 z7+>FrmHp?Sq~kKJn3%?TuKJCnbO}kZk%OUK+3f98_tD5_9sepc8a8}5M-%m?2{H^S z$imCo=n)`J_xdk@e$lzGF0hxRH&rupb|2sln)s9LqPNK0gnDj)>qmO)o+YMiJIy^D z{Vq?9O2}JpRor|km5Zp4ghyWTSl4jfD$d%A+359}ZVtLbRGb`Go6HnC1K)80*+5Gc zOhEZ#a@gLd%g)VCLr1ql z|NClG-))OKy7Rb*>0g*X2fpF8$T+OMW(M}303HfnQu$j?=~TmKBs|m?x(zlF^>|_Y zef%&;Oe&!*Q?3%z@BvU#bip+R0o=AM2N3;f0~c^SsYS1$`lYy|)Z+6>7nkc3+@H3l->P zs~M=ZYcEz9kB&7?oGzN`1TXFtWBY9is~iU?(^dwzPsF2vW;Q<7yTUB=8sLgtjCidh z)6n&H1-5$p#5vvJ^i*mGbS7p%1zAs*>$K9c+-%g?P>HX50$_7vH`sou1=*b|8iJit z8@AiCjHaCf`D-x;-b!m>PP{*7nSF|`P5eb;efE>bxi+Av!f{PsOPGfi`lM^+JuV`= zgfee`6ThjNxWp}(D7*_~9G*0hcJl;!Z)6UH^seR4U0BTi_|V6$H?X2@8%t=lzJRP7 z$mQl*-$IGTVwTA+u9i%{7y-)&4yd+2aj~H3m?(-IE(FjK>O3*ocTYN zYum+%B*~Pi5D`M5BywNpajS$%15z3cwNoikDiTSOIa8FP%w#BKid^S$rBH+hZM#85 zLbgpswNuG^Kkw)H?S21+T_hAL_p!&@J zBJ#1KM|ODcP4*D%7W`jYUPge<%SNrR{7)GvzpDw)9XI55OV8t=Cyd`-eSp?+t~@aV z2k=db#~nU(MEJs2dOdFl`jtGn^p-XJhfnO8L#LJb|38Z_v&QnjEWV78Q2vAnCN@an zL|!g-wRqq((d}%?Tp_Od(iAE``3`rLR>S-pGwy;x0NJmn$JZS{Lz}lG)5x_c=;#+n zp53b^kH#-!z14i~s%9IJj1I-)wui7}Lp&9U-o@&l@WkH>LukdD06aQ=j}%q@q$1iw zxYo`S6;mSNMt(3@ulolj@@C-R*m7zyMV8xfr-m61n?x$)Z!%fkPsx|Fp=j|j1!m7! zLyU^H@%?AW@I|WRiR#8yxUKkt%nURDJ0k=Bi~=WqZRmb@7CJ(E)EjYjlL_BIQ_xpG z(oVEro@G=VF5x-%9Ms+M1AgDofS0nn!0gLPoFcsi!;cB^x!Ms%!ZeS5-5Y>rOSM45 zwia6c>;$p8Xi&^i<)ni}xID{DjOpJRp1*fX zK0dbB<&1LobMbw`{JA49ApOXX+U8Av6APgU8vP=MzHPowGLDtO!#@dzS^C1^*P@W= z!9vDqF__hR3Rix;%Z5#<;^dQg@Hx#3P7!a?WME5d2ELL;H$@z|NJv6XJ6@?T#|v&z ztgrMXvaU^y4t{z^guId%<1B6XbcWKGr{~qKXqyBY`XNLv>0kEX^Z`1n*#@0RKhy*V zVXMP4*1k;~T1C$aSjX8IIp%^n@x`PjoljSZ=|It%24d*(j@qgg(%(-*@XyLiOoVqN z?poHxQ_wg?n%8c{^7S%UV`fW&Y!SB0{7t>oh3OQNM@*uY4%75gtC-onqFxWK{ zpVue>`0A1ZyGW*4*cKX^->^%&&r%steQf*HMs~)tgh_G(>-Np?vnW|`@5xxKY0g}Z zWKiZx4P7b0QZwi4#41~!&T)zd6YvMslmGGXX7R~mIj~qX4VKUJ1h4G@sBujV-tX80 z*PML#L{gl~e4GKfse7n=AB$6r_u%v`9&a?|c^6B8 zucIT_P8G$|i(`nfQ5po#4hOxuUqm-(6;xeW4D0M#VRULSSu|gQ^J>~j%YF)0F{&l` zazoJ=ti%WTatnMoTt}qKN@&gnHC*WS2#nkJLXo5pQGJw%Q7ip8IhQQFd{~3%r&i!- zlnyr#D~GGZ<2bF2(@FL1P`>eBx_t4{7`kDDIPv}*2YZY2sGi0vqJtZ18&0NRNP{vz z(_klh9$n10+89e(4#uF8KqJU0euEyW%8c(Sr{eY|T&tcT_g6i`gswK@pS&i@x4m?A#}cht^e5>68F?Ym|bUkC{M6 zVI1s^F(R90`M?VAc~H7rpUAAj9ar-@!)nAqpLq4D?tOe|2N>GNM6K*)m;$G0!B9SlF{;Hm{-wlYzyOs_++fVZg6NXFlkj}s zYx-*BIXiH;n#}7nfrAfJXjArKqUYmIjXs)T{(uGBp1FqziBodm)ZfI(;~_POC}C=> z-;lNKQqVnonxylZh}q1&f~?AiHp#j`ZPfy7EE1(nNu$iMbSYYbyJ_5b3M>^V0Q;#+ z(JNjEr=I9x$Bs#YmQ*ace{PTnmEC2Xw4TrkOFMLx%b;hMD3fSeZCJ4S8zU^M19JM# z;Q6rE;`WC5)Wd+K&k`H)(Dx!xj!wXnb-~anJD=QZsU-!Liy4JZH#opkM^E=Ep588B z>`aTJTGuxaJ%6|_J{i7G{eazvt?^)CB;M$0C%;17YiC}~Ax~sq(!fe# z+?;w6u%!*x8#f8GzZ_I%S7Q69I%L~T##;e}B>vhLuimOpof87IoAa)LLu?inT_ zI+>^;FGREAFA92P2cau@DJ{=h1#|t+vMj%w+^73z_u zw+D0s?8t|@adfwr2SaOl7~HGH#;;O^m8+9^%L{r*>gO^f%P#R$H@)Un#HvD}S1;AA z&?VKrW%%>EIcADCkzRu!?2lRi^t>b9iqWL<-6f#ux0s|TOM$YN5(dw8rHjAk0u&!5 zy#vv#)ynN8dY>AOBu>M$=7kVw`IP;scM0{Dx1e$A1A2N}4AmT+Oq$e-sPWTdq@$#a zdhYW_Yv(9(pm-tG=-$LnoM%;g-dh%jAC6L~*aU2{&7+x8A7SF2Dw!jH0_PsoC9}zB zZRxd0@=VAVhQ(d*($z@TcTNVOvm~(R!D_s+Fn}z3!=aRxIW##gr_ux7cxki{#QVOH zzLDcJ+aUsKwnRc;dpI+Ct{49iuq**~Gr&8}8S9No_?O>RQp=3F(DTo6vRN{Ox{I%; ziU*SUaelFQd|L?9X1IqgAF#&@odG=EA|VUq@j_x1Y{o0hSA$=vo1w0XhnwfuGS4sm z1e?-BaI8fL4w-L*{%L;@6%B^J&iN^;yj~Nh2>6_qjc%kPUyGGEmc;+S$aA8Xr(ot` zdD>tuf(xA&LO_%{v zUQ69C#=|Z{8Qj)sibGQRAT6m*il^(Nf9G_%;pmWbT7oQy8i%JJ3aS7T_nTm5d5cYI3x5CyBhcu?br=`>3 z?XnNpDbph07*0TYjukZOWfN6jdG2o3Ega&DvKI#y;fAG!L~)EI;*+-FdF68a%Rh@N zZFmnb+6$#h!WUXnYVY+a? zrIWP{|HfW5wnLl6D{yyG7H{>^3q&Mk5$VWYfCtnSd1XQ6uq*cuV)w%WG#Av`+4Xe zuo{x1#p&4Gd`MV1PW9JI;D_KQh-*gX#^O*imal}f-ugh+el;>WEe>488pwdmT{cB* zjEd)~;DBBa>sSznszz}bcv1nkj7-oKol0Q$bBLH+R0NCd)wnb;i`ev=p!GiI8qb?O zq$$P-E|eQ%eWxa@9$Len&?oqAD1|i2ix7pb!<^-O9eRaMMxzi0B6N)TvKhTpQbZJo z?UrFuff^V1Fc5fvxT(_&!+%=lc9&j%v1$2v1>7_H-%wsb!NO>&F;AHIR16yr7AhGPq>4j-)=9 z=X6@G&=~z(_-8B^e+VnVY+EJl>srpKdo3W2_orZB$7WD#`bHa{+TbRaTB^U8j}kv7 zNaQOwDvc9teUKNX+sD9^;A;RyESozkhWWMiE!#Frm>bOOA@sHe0Vh`!^q3|=VAIslKz{?l2!3oMx{)awVzA}TV3#P;>dO41| z8G(ay9%?8`!fOQ=M#a$sqbrK3OX_1%zwa_{#>*zE>a&^dyHHMJ%UQZAkOxXS*I9Ai zE~-)zNxbD#(OK3M`)3|v3!=5?vCF;WNOvo;B6iftyc#aMtigE?1$siCqQ65ie&6{7 z#zth&D0CmW_p$)BA0|PpW*Xb#aD`~jOT$2WNBHKY4u3^fgMYm=s`2jQQ0X`J^JQ;x zad!xsbTL@wQ-|#6YGSzdF$wX~go0FQl4kmeUHS7l*fhw(q=ReOz=~$ZW_BE7Ra(iG zx!C|TK4G#flgQY6HKwrg1NFPzMbrPv#L(J7{E;RH36p+Pmlx?|Ur7{Bk6e%a<`E#4 zt4iJu2>#9slOQ~nkE$ODi9Xta2|IPsFy9T%@y?*gk_*wcWm5>NNDq94V^%LOyX+p4JAtC6`48fjkAF|~2 sabhk%&N|$jjCTgzYfcCyljBqRvFcGS&OEyKzg~p5kGDs%hlk<+07`hMzyJUM literal 0 HcmV?d00001 diff --git a/behaviors/custom/Walk/walk_R1_R3.pkl b/behaviors/custom/Walk/walk_R1_R3.pkl new file mode 100644 index 0000000000000000000000000000000000000000..5e56c0ec9fa07ac65c15eef728356c8e17dab47b GIT binary patch literal 20820 zcmX_{c{mqu)c@^5cCxl8QCX5U@tOOKB}AyClr{-%Qi_U7$i8nO*`kO-C56x2XK0r~ ztEjX{CGG3CqMzq^uIo3~T=UP&{4wwAzUQ3tI`4B(B74DNk^enbXH6M7lhn5b2W;PK z>Ju2`YZ?%|b=M~EpdjzPSxKYU2Ko8~ZriylDA;FLR%TX`!ndK}J zon*H-iL`6iN=qoathY4-F`Bf-B`3$G}%e2Y!{dTVKK_N4H)OqGxog^`CTSZq# zZ5Ap#@S-b)s}nMX=@8#7ZBV`xU14RP5jM+^2{ zpxyqXAhWrFRn8(2X+xUruL;$=awqA`ys)R7~k%N)|;h+A$RvJ$?}T1GWL98i7ICpSc%r{HXi4 z*JO|VZ5;mHL=@}Qz|(XBeIsSeKbD-#H@A$1nWYkJ;0rUFwBeXw&i+VPIrNXd*xyS_ zvltBdsKRnbf6p~v$3;HVOXHOxzRwt5Qe|L~F8XaAzN+S-`$ zj~CJTE_mNmFz#*W0wpKLi& z+`=&fi!HfWX+zr9u#T9no#uxLM3h+;)UWkIrI5 zROiu)m48Ujy;|-+mlU>sMIhcE9%L5oSqwjGS8{h=HxLEKkEBOUlKm5Y7I!XoB2S7& z;iiRacpCT|+7jl$j=&;tTQ(hjWmv*JtrVE^rxG*+qA{>Dj_hh1W~7eS;<`WMV78YS zIrl+?&o@ZNEuV}bFRX@&4tZhvN_G6^aDW?ZdP2mN3aO>%RnZcY2;#TR zWcp;EbK@t7&=*td>GWqA@KV;2TvpQ(o{X4A^N)1VY?VbAJxUj5Dh|TlBA)vZCBW1B zBB{}IXIQu5hA=lhR50n&G>{rK7fz2^i-Fq?;?lD&@Zx$Omf$${%Z?7b)^Cmi_cVHE zbr}J<<8&Oa1#{lI!s_}Je1OO$=F3!ldPFi1C8CjI#J!M}ZRVuk4l|;^Khczw)x2r7 z32n^?WVS~SaFy2ExTvbxwC=bvoAkRM>vqMVvwt!EdTR{PjklQP_k78tP)U$^ri-`B zwP3(ldAEt$+X7Ov*$j3tpcbWHw!wCC+z>aardHxX`2HlT*j9x}W%3FP)%Vh+U~A>1f;92=cQ`=2kOIZIv9|AT-Xzrdfd zYY!u8>s{zIgIq@Qq&^*-+d(|O3^R|DJV;UbBSET8H}$Wcj~&6!$h4ljWZ9rCSzEr1 zbcOWLiI;=8x@$hzDDZ*>*Wb}AI<2JHA6P-PfZj1_BMT-?;_u1Cvu_u?Cc8qqg}&Ar zf@%kbnYQmMXV~frpY&r`=AR95wCE#Zj}!2e_axk$#ObHY1{5nnW5~-!eXaq9l9uHHNiRrV&3j(<83Prs;5y0HF5AY*#bZ86mb8r4rd#)hKfZ<^U5+#F#6(A{9eUD&0|TJ zzF?T{*J~xxgE3&W`8#-fPh$OwCW3{pH+=Ydow=+#nYgc);-8<(<3EVKL3Oj=xX&UP zx6I5T{G?3K`euruUMv^KWWrP3e&K~JwrC{XOctCSM-#pTz&1G}$lSk~{8B2Rn$j|a zpMl737IKEN4`BJnDk|>dN)1B9pzXyalKE(^Rc(|C+bn+ze~wqCe#9O9+TOzM)lmo5^1t37i1ysCmI`{4S%31zY7%e!Vgi;FC;J z1pf3^&;Y&bz6p)Q%4r&3z#h{&&RhX=+P@-=JdQACJ|2|AlY4I?|7sMo*tUdR-Mp0% zoi&}#T(yvU{pKr#ijD`ZTtWs;7t#&y9zoKPJ5(-T7W>B^Bz4=4(Ca3}SoJ6h^?%yJ<@KNirO8krrO@#D{x?bnt^bvAFEP6&*Uloi9p6 zm73YqHFOQhb+V@^@zK<}J&~&#wUqo^K8=()iPITt+G*U`zl`bkcxY|xB?h-`x&5lc z+_j1Z8kaSe79@9)ADJGQxv8Jqp0bTA*StuESDUefoq%S`H15iCvl zL%f2sh-Kdi8n&zh?bMg>V+3#D_pYZTsXPMZ{?3DgGMe16l~-W1=O?QZ# zSDwje(*(H-xlMZB2O7EtU(l-!PPp`%5W_>l$k(V6QWX$OuE?gL+@G0Pf5?Z_CCb7i zmFXC%X^%$NtRNuuAkpjZBZF4{aQ~n$J?eIXj$gYL4p@)JhyR@L=|o4k>D@g~jJ5!hU-_F1-I)r; z51VMr@D3vM@?(}*hi>1Ce6XEYp-bite14pS5aQ2NsGO2 zYRZm{KLH!MtLd9s13^&ESx&s!0#_v!;{C0~Xfikeb#+Hc(dm9>gZEjY*E}1SZ<&bS zcY1Sy=j&k5w-lu`3aHGADeQ}wOPnyWT_9!UNy@VwiI$H)k!fk?R{ZS~m@nT$&vu-q z>u=VR`3Gbneqk?(JvbHr`xlSyDGJbgK#S_17bUWPM>L(DL4M{rMHru z#Q#L$orN(rrjCOU_cGQ!O&RBn4ki1IQiyp#Ay+L@&=fB%iOY`!^D+4badfhovCY{gKEgHldlGnMj;xSahuZxz*>qEiz$)NLI6ntZZ zT=1$`tn^jlGd4-H=eF*}(c-SOJV2Bb*Y^s#?dv(5zn9sdd=wt2#i09BBg|1W7i1@H z1ltoImIzF>R_Lw|3?-)uTMfndk^k+9H{@)FhB|s*8w?mJ+^`-bOoptcAA1 zx!i-9skGy*0g;N+5~@9XLxqC_iSxk6mKIPT%w%Yn*Q} z?x$QCO=?e0N-Hy?1~}4iIFVmDrvpTP^kc33G7Jk_1~*r%MP;dK2scXMW_Yk96UE?( zbArI2_B@U)8URntwUETB!Vd>ml6}OEta6zE2ltL*J>Fzs`R72a2`YjLWm|f{O$zpl zJZBaxvY~HE|3IhfELK!^DiP$}BmyNwlQUh6%*z-OC;_%W?KCi@Pxy(LSzHmb@2?*8Fvkj4Ss@@ z2~k)kbRn=L2Um(0kV!;=vkKTt?Js$QuVg-LIKPB7``eF(Eh)I6cMW|hID`^S$Fb(I zGDa>vjawUMgRtNl*|)xrD{8DHeip4z?vO|_yyb-r4?p5g#rb%=)E0ksZ$+8(UV1(u zn|y!1Txgm3iu8Isr-|YZ=^V3loLk5SvT5Q{dVWY9<{Dfk?VGE)X}c0={ZJg)H|aiC z+xdwJ^~fb28H;F*=t6FTt}?dS^mB8+A7h>v&Z3Fa10Y$d81q`L;I;hQxU;{Hr{Awp z{$dI3F!dytbz8V|=R~-iwGTLvO}5mqGK;X$g*53y6!e`hhnTOa(7(Q$dKy{N>=X0x z={61azI6e(-h2VCN~YksbDCI^G#BP27NEEP5mYXX!gdiem~vw*r0+>&u6};RIqmq2 z*GpE@Qy#ej?*pc|B&Z$q!uF%d{XeuRYzLpBkpgwPGlVikoe>^wrv-DgQLV|J39sbn z=_50!ZP;znY%-om#Q!B#pQ>oO_j?-QA4V#U4Kqa&`SiWcNV)&?4w>y@hJDM_ac!#z z1csGhPog)t94o>J+Ph#<#AOm2{)x|<+{4T`7edUR^EufwqQWf)6}h68T^P4+ zHC#`Pz@fJ~uv3&BLU zO2}~k!aOe072dH)B(aetbZ6NxHCf+B)_0z#4te#=rtn!z;VwdZHIzZ>vKZQ#E7P9O za?Iny%c=UqxujifCppC?kTZvV5|5EJo7=pK{50GwlsJ{Cmu& zJH|2(MFyGgf@0d{qf4)>9$@&fp|CXOHk#-ygtB^Va^s?naMBM6tV*&2n+>Y8-Yhxw-Yw2J{+~Kn(|W*MdO({T}1BpDWPZWL{PEzqT@%OrQ>^_ zZ~^u}V#1$tL8mGNSFS%|4o~!@)tXs!Lcs-ry`u!toHv!T-=j)APyP}(=$Mmh_s27n znsrHDnH=p+Oe1Qq3*m;!S^nFA2k*5{mF+*K$0i&}rgNN*a(~O}Ncs{92;X>_?6CG{ z-Fsis!hT)$<5CNDolyW>i%o}rr-Ethun4T2n?u@{`BNJc#71LXcHujHer~uVYf(I% zbb0NAfP~F-j@noHw!jAY+v`AeV;uDn9SxtizoH7t4$#`~NtXOQNcGRzV4&O(H8>gz zFDj-$%JIX(>lX>~zAfC4){&-_%2FWFF%D)tN+SE8x)WI+1ttSY9X9$z&SGTCJ@dK8gNlJpL2U^*29dh_3+QDPLTL-EUc~^MMIjD zc;8FfG<{_%Y1^z0Q)@$sS@|8N3Wrep-!%{$ufVR%xeEcVzv#(Bz2xj|Q#6^_4y%tX zqB~AGfwSs7unI_q_{)bO!bk(hPdbLkNLh85?sJ`rSwHn~Z1*Dc`16pfmMCP_D2T&{s8RH+>;aT`6@`{Bi%G&y z2YCK*gr6L|LA^e2fvf`xAe`BQBAv%TS@a?He`Bfj7fGTorV2C8N76$x6nL3@Y3x%! zi#6xe$f)E^5TSXG^NL6yH;UCs?iF=xeejLWl8Gkg=@G_nWc_z;Rw2@V9ybPe9-$|9 z7(i=nx*$w=kIt}DBl0EJ1%LcTV!>7;vSQU(l)GYpAI9{N(~Az$h53Z%QeJ}D=~F1% zZOR^O&SH;^(%@cx`Hv>QnM+~|?p-&?B6j}QHTv9Hf#%)n2^X4V?~@L(d{`bHZ9G%TSdMZgCR zUE;=_jDx~Sb)-@yh3*oHf!Wc1X3@&OG<@Yke36C>8Onb*Of@I<^kqS{8VhQmBNTHNl<6zPokqIbu@*+6Y-d9?7#J54z2`Qh`1mNa z3uvR?!w$nFy&^8>OD3IiYAH0Xu*B2!yD+RQ3_cV^z#^qWqGWD`%L9)R%i_Hl+&Bh; zs`82NoD6uvRlyE90vEaIsC(}*dA@BfS9kLx7rA8(`)NT39dHy!yYoI&v~Lrg+I|Rj z+FL_od;@B9zhhGOOv3asO(N4?O~Si2UrT9C?HsOl>;{1y?E7CDGfOJ+q;r0dT92W2 zzWJVlzh|Pz>wxFTeGVoUZB!xHe<69de+#kd?57V^W|Q&^HL~=-B2FeekN#U{Y1{us9+`YBhK_fAk7Ii18$&8B;MFW|nZkH9`on`*?j;6E!5v~n(^ zjvgxjnPV^|CKCVKe;zxUc9ZP^%i*k_9Upm59Q+Qy0r?RopC-Bmj=bCrr|Xp1=#wiz zVxuIU-JUA^{y`F(S8t#nnp&6zj@gurHsLjhGgP0}M&Xtu8c;c&UtxHi5!W2~K0yzy z>t6zgK?D@P20~A?2eli!n>6opqIE7BAe7q$#Xg$ki>(bz4Co@!SE8^lMG9gTS?YY6h0Z#nFYr5P<$Wi=Bx1qD?Y>SNp)~p@g3J~tx8i5 zR^!y43Sict#&1+Gg!iNG!qfGOXw$Eb5zS^m|8`>>yd{dOOhR#D^&Hkgb{zYyZ4uoZ z_L$6UO6FD$YO?V*o5^=Jj*ji!4`242rdxI^QY!R=rQf%c%!9VXbX+98G9XLlUjGE0 zTM9^>Qyq9eo5W^a&jh7~>$#U=essbJLz8{}ib(xbBEB($)OZ`EPq>Y=Sb9GF^E?8r zqo>d@k+Vqd`33Z_<0jJW)JNY8n8Cb-^O$40>B#T$f}E3b^oMpdQCZW1*T<}cjGN{- z!+kGa7-I+t-yeD>IPaO6fRURjfd?Pmv=0_S^l zw&G@}o%WTgn@#3_2xqe~f1bd+qpM+E=shm>@JqT>>@nG;k%wQ318~;Tg*4Q!2}(1Q z;9uZGe8r1$t}jywlQ~3E=x7xGd5dX(`W`B7y~mvw1JJ1E6}9$Hz?U=9Fv zQ&W`2s2q7ASmq*&r2;9kE>aqf)=ve}X%3M`#n7#2JXSLmjLQx^)RQ-d9fxKK{lNoY zoLxxfgCCV^SwhPGjnq?*_}w4Xc3n7(%7IHGKUA z4=~u7!!_)F4x=*PQRx|Z_;yVOIps6LV!A@%>)#C|>ars36mvyyt3)cXqL=z@lVl}c z%F^lz3;b_c7s+@Nz~g~RlE1!``qk(#U%V*ud}lmi)MR=2XYL^T?Hq}|d4sg?)&e>6 zzvN1*DZcsd3H|C*Ku51S#mz$_>RoTcWNf`nI77f0&|Pru_9ZUs#W51k zX`{tP4VU8$= z)kpAcxQC`c>%pU;>xjgJF<9k!2;A?Tg1vKQ!Ksm0s#cx>!tuqpNG=H6+zhc+$_{6$ zUjQXpW%LV4p$Eidx&HC9$b$39oVLdyE{)m2UVnELrleh>(G9(L|Lj@9YI>7b)~Suf zgLBEG{Q^?Rt>flq&xb!RGf7c_A6lz5|K%30Ps)DqL{sJPq#_!u%6UsZQSmx^BWR=7dKL znJ^;@OdaE4a-klpS>^-*C)C(hsa!lNW67t8Jm zhF1!6A=7mXzue;xX&j@#9_kQhjf;nxR(1Q3JBLog@elg^h6{D{(A+{al<4GwQk8IK ztw2z{d<6!Z=P<**ZQQC!6Tpp|#fN6)aItQ`=%~c6u*5=w7$<{3H8mB6ck02{ndan{ zs6Kv4ZeZ>l-bmkRr(vjZ3_Kk#A!s=8kURYS6WpJ41cLReNJ`i&I#yQ`a-uIXN3+(@ z{16>HC^kg$hwgE!#soE0s`k+&nE*zmej3-VeS(|BCX!2c50EPwM~Ea&radvrT-q^* zruhp&*7X-FG?ZDRue z)RXBEKbfuTim-gU1n^!yIPGc~Wek8s~)M_f5>IrY?8Nk+E@f$Y=W zwA#6gUe!ou_Gi^Ft-1f;MCUqi`%eU|9v9QRg!f!@u>g+o^RYtc$rPntq*PLrg!t)U z_}mv%&a#lsf9;1`g;Tj5mA62ux{LHiyd@i6*AspB<0SUX8M?*Q3ze>AGS=g#!C8}k zlxjM_s72FQ`Lik9Oig7r&*Lc5W7NrLjhajMlv#1n>65XfdIxH>9ma}06RI3A8MlF#MV8=Zag81pFMy%*HF(iOhxcRmPJY&L(7N97A_&jb;NsII%Az`e>|qH9THeif&8364UK2 zj7Z&m#<@M8+vM3pGJXG&TVL0aYELc9b17xa(?3)9uI1d6(@h{VAP&vJ?erOIh?{4v zA{A?G$V|0hFs>hqj=`2-Pxhf{-Di+du_hG{JGkHBw>yl3`+@YmE1Ai$njdHsJ|F2by=kl)4waW1c&{&kc_vs#qyq$`bzoIa9j3|+srUhHA)(M{+dr7`Ds-Wp% zJJ5To2#%TQuxGCo`YlZhN@azkitZD&NSUS!gOdgKbwxlb zu~}G?noWC_{$Oq$KEqg>g>dwO8NF1&F=cWW$;Zvw#IZ}AUNU;jERJ&IPM^Fhc;-1B zkJzr{$1K|qw!fA6vlj9Eew`?AX_R5^kJ2X7s!TcE*A-+a@*Jj(Pvci_|BBhwqsf_S z6@HJn1Kj#nLqx39qe+^$wH-5yR(ei@WpA!Ck>du)aW!4$tCK&DQ5qym znIvkySB=zI`_YM7UZnMdFP4roLL;SBjlvsCsr2)5{IU2m-?g`e+cr%j2sqnc2_V}RZ9CcY)Ko{Fx*YeNuhO;HatHzRlZByu+4hJHvGKRF`!*G3BD@uy` zLCEw1IJO`I4K@Zs!kS^4E`9~eovY|<8F9AELzGo(*h5E44A3F18}NRq2}Y${qmT6! z_@aszM0wOordhZEeWoR&y1@qeNM0Nx=S^d~q(oS_IR@rBYrw;MDwz7T3sV*T6W;0Z zpbuBQhKQ1Ux@}c4C`&}a0^^BL*F;I$XL`I88|I!cI{;b!@aoBn24DCJ~ z$~VbMu(Q2l7{jU|GI7Z!?1=cw&0Nq)I@Q+D#KdffTJer!+ZC|-oi}a|R)CuPqww*- zNusKKoC%!eK^))LkvgSnlvCYFXSYlMYxXGI=T<;@cq<_-ahQ2;73{P+1;gz-`RuhS z!Z_tYCbM%ke5xM>A8l%}d-_P9de9mzl8Z5Si7ZB}O@*^z9aMg)G0Ryu5P$3IxL+cQ z+HR79YjRR-+OHMD-c8;hUoxW2H#=%Nq`*7of8%0nNBmU_CgP?Ky8QQFg`_`rB{isQ zfHDnja$9mfF15Qs^yhuS!)s5`;Q?#NJMkaC^ zC`KNf)FP{vO0b*dPFue45P|ZqEyVb_COUi1gu=Lb`f2TYL95|_(EZ=DA0wD5&D zBFQ8#-wS!&C0xzzaN)A8K45esh^)QOK-pDiTJS&zbVX%g_F4%N-Mj!6v!@B{F{c66 zE9jF?w@30b7L6C>(OG(S_@z3Y9Q?Kw{@vIQ;-|InR)jp}S?R&4mOMH=w-k?fj=|D> z)^Kg}T*x}EKqg3tgE6@P>n8fME&Il@E;sbps)k&$Wnn(uI8eb9uenQ-pPJC5F%Bbq ztOZleX5qxU()@h+O!lzDdfa$qBX-y8!_Kui`1Ox0t9g1ecC0dn=8Nly3wcdnK0Z!A ztA%3sA2p(8E`z1*iBNt2B(DB0ht64UXmeSK@4j2e%{H;8o(XpJ*}Mv(nlDczT&F|a zuivD4^Djn7Mv(rD22xc@)nyPULrOE(gb!cvSu}4!<1R#kCa6Ql;$@Y<>I#tc}m& z`*k~+!p~}`6p`mOt?s`9+ zHEB$sV@s7#c)*leo;0TXh%Zb=dIq~!QVcxfj={T^rtn2=BDuLKmv-}+MEhqf75l9N zr#B@D3;)f8-c#fOYE` zF33@qpSj(Yb{*Gb+IuAUrfXrab@BuVPE}&1mumCN>&>WpWefArse#V4eoWQ7i^vhp zTC#eaAzAkJ6g_<6BNt=vf!5`4OzOBt(6m>C+Qs~%qVv+|%Qg=ZJ?kTv`}QGGS-A^s zb*E#1QX?}^asj4q62XR9ZNmGC^Vu))j(nN^VNP0823S9WL558?( z0_1=w>zwTaVn#W1RDm2H{m&Q<+is;UV>Q4=vWpsj_aWs`ZS?qjcarc)7gJu0=c+H< zz|hcs_~f+*1zD4^TUUvGag?XErOBi_L6~o#dG7a+IuBf<>X+iQxqK6 zd`Uj%jztaIC7^WfAl>?6K0LEe#z2VyIDIXitm=%0eLvE{?sYc#GUC&zng1HU%$EWq zYRsm89PySUOR;^s{Lp-x32p0{4L5eqg55I@!gQ-Se0`-BmY38*dE9PRJH8Q{#LhAE z|G8j>cs1NTSqBq;&*ojU9Wi)$8+noAN&Hfekrl}q$S<6X@&1*hL;D=3ejyLLU--g< zl)3DcXGxe|up2WrEg*X?<&!Fp1d20na+|-~VXBoJ{`jH??`Mqx{ZTeda4bijSIp-+ zAFJVkx1!vSN+*H%t~heahQZy_o#4coY{8?4xww4&7MvG)3tcyJsDCXJKI%Mg! zi?aSxT^o2WZJ(no(!wUj)q;CX8g@K9xL=SQ0p;}juUUloXRhBsQ3oC ztN)xnytj-_9I1b3jxD*=I|(-JIYQ_6Fi<|p8Ves6<8s$H`0VhRd%S%UEf_un{c}1< zuf%s`)G}dF>k$31Yz8Vks3a|sQIO@ej92!}x=7Jp%=>M4x|EGbQ*b&{6WJu9)s?~>~EYO}Bi3@Zu6J2wCGTK5&>a;g8 zJMJekf(_sCk-k56Uu+?gGF6;!aXcek9Zv83mnmE`67zlX0)-b#V}+As(x}__3qtPY z39hW{8?nscDZTMb&@LRY=pQQ;?maX|ApP?K(|KSO6L4oVeXZvMkzgGE)2eup!cASwyz46T%~Y;qMm!mwMI%xse4Da``B3)<>Mb9zsV? zeJqgG0HtL*?5l6fNm|%Z%vh+&{t&H&i^iu&ehWb0g0bALqOCYMGXrOo2=UnBE~G7y zWa-aCmalKb!nI8|Xz-(0ZldU1c6*T)|IyJ6(_Y5$HQUXxCv+l`kPq~ag_ZDm!Z_mf zd=cDiI?qAoQw;r<3s;W^(w%=?@vnUk`J#M+CXi@u)U7k*rDq~tSaPpv=KKV!am|P5 zb8f_6Ry+-F%AFo*)`w&BeswggoC}a@M4OG2;K`;aCf4K?!lp(fRhIPW951Tx^%hrU z$FfHABH6XQ`mDKY4@g={kqwr$@ZW`-BefEcB6%wi%bkhqjHBUSawfc(&La~s3Qc1E zavy`dz^>~JS!DK`GaCH_SKqxv?e#P1T-7qV=i_B^)H?#tWSi2_dWodzLn}@-4dYux z3^4bA2hCWqj%{AOhKvh1N{&ALA#hN?ORHXm(qDe}sKbhjWN}6j4R~}?u>9K(`f)EW zcrnqI-m6w7i~jq>6vo<9TUjHrezqpuj6OiyZBlTG{yaR=xCUcO5~2NwH7ZQ|Lk7l) zz}qR!D3Uvdbyn8`dG;b{8V_y__>&O^1N_aN-nrW^g_AiAN=`)@DH*YkT_XTyP^qvpX9QUc%#O~ z%}s>3S@-DsN+oQ+a1Cq!+@SQTK3&=RlP=>Hfy?YYbfcO8&($zg{nTS_!tV$gTKb-I z-NwSgb`Qdj=$4rBm=3P{%gyVy5W4FRFrz95>DBmhI!h{+)cW!q)goNpw8OY~u^Ie2 zr3jOoOPPqdcNjr|GpFk{jl|qX=HT_&yxk%(C`dP8F)$lmRtVwjoN;g_WQYi!DY4tb zo!EnH7fgG14EwB=$!<+2+^O0IKh|!9rzC)z>=c2;|2gn?4Q*kq_EvspnF0Iku>s!M zaGTy-FN#5nu2bt#sp}Z=dhwbWu+PVcge|P5U?IOVp$kTz9?wsR4nt#$S`zD? z41Sl#K*^u2Sf05CSBsq`9gjz`USgGyvQC-aUL?X7IQ+slJ9P0zzy;{uA#fX*s07y<*f=BdwsI;6KiPtv; zsVUavrJXxHfBz|6bbk>(vTJATzf2?p%|_HUE|;#BQ76{Yrm#HhA$PCn294i78AFHD zXv3dg+7M8{G+Uo$_J{IdT=He42c3mo2DZ58k{;iG`7za=rbtuTv$)dbkz_*Pd!n3e z!RB{I<7|z2JnuUT3%dVejY~Y)FnkqEWcN_pEHfH++X=~;YdFRBKlJ@w3ATfGaLFnw zKIU0H-d&>reN|)GCsKKsXD-58K2zjQ=W0Tti6(Y#45!~}Jt46uncMXBGu_0yRRM6ZIucJ?TjP$M|hdqWlwU)^^YKYZ!}dfT1`6*eo~D_ ze_Xr^goQ7aK||ye@o?H1-m9_1u3NB{q+-<}C!$o8%rvB%!d;!` z+&3i;{7=b;`b$NS8|S2;bx{UgxkiQCF%*DLo|>}G{5baWg-CAOi>r`lo=^JoCeaNy z=YiXU2J#?R0()3nm=IJ$H;i^7Gd?8H>I_l*ymT&Bjd*<}eq^HYbe4N@XdXQ<{VSv< zJQS4Z@J#ghEc(`bJuZ-XC>)unqK=naah7Wlzv!P8wd!=BYh|SPB-b{<*SSt~@cwTm z?p8DTquIjTTcS;0- zp8aCl{575Kdzww8LO#>2VL9Z}Qx974u$sOz8CC|!{{nn=18kdm2SnUaYS zOwUGHvZNt^Y)qYqa|deZrNU03d_WKL@zn&p(?HO0_b!;&E)LI&UFpV{abWPC;&-{( z?7n9OOlyV~{%t&|n>1xA?Fk)2kFMN6ybO7|O3$DFoHB=vP`1Ld=!bCc(lR=3 z-T{>QGzwGetcds|1I+StLyeJ{qAL;Ga88vhAO2eb9Fvc8X0jIWS5h5x+=@tMMln6; z@|zCLa3o8#rjsPcQOqNo&v-Cr8P3w@OI^o> zdX+{*zEOud)W_10bR#a~ShL{$uJQ0_hZ^1u5Tyne-_R-RO(FiGC73N#Ch^a5xQ;GU za@Hq;#JzSx(PviRC!J58P6*`0`%l8(z7^OUd<`00jamIWmqAB&4nO<3$L#@=^yN~jK=UgnEVW=*)L z*n*up`Z@aZ$7qnC6v1c?7HAZJPG|^hYii~;cKN_%!F~+fQp=RMTqTL&70lPvLi#sz z8SGQ&#G{wiVotUxgcyv&$$MScKS5)N2&WGJ9S|d%SMW^R`5s|K#0KjAHj6B4*MP`9 zk(^Gl6&Y6lDjbZAgrA*iR`fc}ABLBIL&f7ntd3Xsu_^6Pp+BHa@f4)SD_t|01 z;p?dL#|-|C{B!$!INGm&gO9mvj$^JQ;li6abf{mHW48(E#Ig3ULAil!_;ZQ$`HswF z<@&&o^dK`s;W7z(-bj~buEfh19^$`?Q_y9x3D#bEOxPEWaO%rtI`@V=8U-zY)!tPw z^m`fUwtG(RePsDXhg$JU_dzn|g(}ENnFGxo8R_iW51|w1;%sIsUK2p8y4N&ng!gNRX+y^Xp7unAG41WDBwRm1*rq=fTyidxOWnD2uTv3udQ1#= z*u;|{#)$gUox=wXbST@>MnIN*T6L&t`lux5A8nQ zOVH>GJUt+w&y-7<*&i8n|MibC_LxS5em^knNDBAKH-euuZU>IbTus~Nb8PE-6$rCp zX};%skZRe*&-!`^Prdf!tWuETMSx9 z{^=I0BC%tjj@;KE)VO~#Z8=~|)n?pg%&oksht)Hh6nF}k`bdDDpBnf-QsR;)X41Wf z^T>gB&2-1k`;`9p262#xg%*kg+K1N}0;u9B^PdAJ{Ote~d6&Q3V_S z9)!nt-RRY*ZzSxz2UP0$$G>ypI8=MYy>Fb)riFCkJx=c#&p1r3RCgTO&k=)M+1C!~77 z{hHZW{$eEWbn2s~-EHGfMD)eu2;Bplu*k9&lQ1l}Qd_Ei|3i^cI zx4nf3noHr?xIDDm-a}T|bU;OT8mxb&#jBMWLFJP_B*eUr#K;>Wso0F0gFoQ;J)(Sm z%7~|_JO;?4(dc$Fn?AUFhkg~!h8W`-V(qDbT5rCQCmBk7-uGO%?Wn-_-N_~{M%Bny zedNORe=rHUW6)K>!u#If78lc6E-uP&2 zGjr+c1u$TcM$SEsNjjzQr1uzVk3xKULIC^Q>@Yq3I*Fen!~Rn1!qxl6Q1=I>G-$?Y z#%zHqEj>3M2b39p-poK?W{5(Smkiw8=z{ws(>Whsc^D2lOiXSsMgIshOu6}pmQ3_! zt)<7ZuQIN|i?@nw_8~Wsvn6YGg}s7+Ttm%)XKh zWXjKIY8R*u8V-`U_-G(@nO`U8A0DNKJL^VfYlEAvRCW-#(aS+gwg5$hGR&^L5fA7J zX;giCjucvU(l8A*KA8Bi)=sMbuZ=VR>v4S=xK@>lDAGzwn^K}w_slJnL}{l`S<=EO zqSPsp4lPnhNQgFVl91GW&Bt~UPRbq%C1kII!l~o?d6wUv=O4J{muu!VGq3l1SjzEw z_Fsy8hV-Tt5uy8o#diz-uPr}Htj?>X&6%ZKw+~}xSc_zxRf&zWpgM_cYAKJSd*Vb} zl~hk!A5-{=dz|rf$#(crrZ1uqWz?dqPR+XaG4Qj`e+T-1USBZle_i}h!-lozsp7}- zEGqS%1ZqlacsZLR(4ae;J9=4{s^VjDW3>$?-${mz=gV>GrZqGeG>kuGIgy@69Tq0+ zjYW&U>*(8+jiT7gadfJ1HYqL%#^|2A+zOMGB02jz?CIhfFgdP98Rk2=rRla{-q?+< zrTg(wv^|?MQzYiHjYX2dVsE2%C3hYWnCAQ}Dxxbq!KN!K|yKPux`x9@|6)5{C!^cL9qN_5WSk=RA zkUmXMWF6PbR;MjvjWs%azW#3_on`K*J1L#Zx%v+7hn{9pv6_6%tCQ^IT4ote%28-7phj6iyT#|YXep+7TM)-`P2|?ck=j~!Yxp^*GP0e8Wi+XsU z{#^VO+b*P9{cG>L`WmeoX9DXFer8*>BFRW`Grltn#OM_U=p%6tFMZcV|4NSE9&lJl zFuM(tUateolVXqevlUuoTT{u zxFrSnVeC(!HOJWL>QP*>%NkgGK8>on%DJceazSo+9yk41pWxV#ilryj@QK?ic3*!# zjJcAGSy^6``^QyQZ47jz()r@3i!StiTCebDw+SoLIS2O=PP6$sj72smGsAbou(e+b z`!t8xJ&nCWz}M@rZL}n5WU5nx*y9^fB7p~j)ak&l`OGVOpx(1^0JvSF@WI4Fm}n_M zZ5Bnsg(H*Tl)^e13^>CqA4#INLImt8_rj%aO~UCo1u}Z1Nq?n{#PNJL=kMzRXXWQn zghD@ST`Egc#5R&@zYcZYJI7QitD(bqJp7<0RyE*{&Ag6CbJ$L%?f)3nebp#jO6(Ge zQ)u-YOB|h<3GPqi>097AO89FBvH1C9zPy*kkBouviN5IQt_NGSj^Oi-6y9p01_r1^ zf%RAMEWwQMUo5ENB64G&-K`O0!7;2ruMtGm%L+m7ZryNsy zdHDnxo$^J$6S8C*w%LBi&fQQvdK@-+ED>5%R3Ph%8E;k>PhtcHE^o{wo6ggeBeR4p z_%Ma}Mtot9pr2i6Im@;zaArm=-B3rzDW_rvJh^W!vT_K=_bJo)>-rLWslzlrE%ZGL zPnMyPVX`>t=vw-)?*%*LvVn{ftVnT368-h$9J*DhQI-7P6hChOAGqIW_-3^TEjpD& z!9kPw6MmZHy-9(5a?|OnPYK@d4-*~adLiVuZ!BTv74*J4j~t{{@+vn^Go@YTka^ve z$%e?`@H5-UuI3nfliI;1~IP@Hgoz+F2E0=&`V=S%r-9x*_7tlb!1(ECPe0C{66ASli(AkQ1ydmZa7W=&g zUE|UG`WL%#{K9M0lpsNmj*2gjJe@$RZ+v3ZV1r!1ZVaosNZw_aFkd$h{$8ntp_^W^ z_;{6jWVTnp#@lot-$nbN9#BhX`>C&t-&;O{+Jr1I@O_aWMg z`L}E#AJcb2l4c1G%xeQ)N*Xq(yoFv@ZQQ1P0H=R#fS9NIY&s7&IVk2ln ztp_ek&SHIqli2>5X_W5s4vsy$EA%fs2fsZnz|QS4ICrcno*FKVA0pH0N6F>jmBMOv zZA~gU^{&8j>6P{#6Xb=bpGt-1#Kkl{9(~=%{-wjEQ{@m*dn#5#rimyj2lH z7tJLD2@}EN`52}?tPQ@DX+p;_Q|L2O0W-HbG-Ny-`}exopT6g4H@&h1-}83hHF*$p zS7uZ5V?~&AMvAt~?csg}NU}ek?Phz|eSybik@zA+g}o>|EVvghCx?thK-T{6c7#hWlUPMu+NC>>}?z`%lfOwk==8qHF8(|8KGJP)A^j{@#$ z;2{4mESzjdva^%XjJLxLTZ5r_z6otKFvGl;G3*F`nks+wuoGj|c?JDa~pSmKw9R z!kwU|smbo$P$2bHOND)3VyG3o$$9l?g3*)34EAD~4 z!T93jWhNu@fvq}p5SPfMvwxg>xli6^7!uzAo&!%&W9>9hPu@ZO>#E58@e>x2x|5>1 zkJ8~A()@?Xh&9)~vpb>Fu)sr;+I~#OIqv>wG&Y@7NQP?555uh)a-y3v($H;|H~#Z^ z17^J6%547M9hC7{cqN#Pkzuh%-N|X3H!i&( zLCc-W*gA7HEUB!rushDX!-*QK2LhCe~{ zRu39yy%G8buMjGP2&?p@DC4FIZ=5)pMlN4WVMs;Cg1e9tx9ZM%i$tv{H1j5R&| zah%>7h%md`LLm3;uu$0mZJq4N_fa03;F?Y&hInfEIi2YR*y1loT}(HY6rEda!k>%l zAqTwzn&_l}BlorQy@9c-eW6M6=I?2PKjeX0& zz&DZYp6iQhKU3Mfk}A$RU2z+Tt z7Z%;Q#f?<$fgg>ISZ8gD^1G&D&FbaMB{Ysc8rG8aMrSh0`NC~}eo$zgbX3@8i|EOl z;lbCTm|%+a358+w;_)0xS$>8&EbfAy`WEQhs0fms4ve!a1%(4{bk6*0gZkUE?6)!M zw8L*F70Bn&)Feq%EeN)k$akf<-RP8Isl~k&b>4)1IG%m#-B;bg2qeG@ph|4(;q&+8k1jv!P#e@~J3Dl6;%Y z@Q$1bEAg}@IR^zgy7#^Ptkaw6oU#jqJ-TZ@CLIg3Bjc1YLt;J z0+SEUU=> z%cnGHNE-C>{eI8+-RC*aKhO0%=Q-Co_jSLn*XwnKNgSE&F7m%?S=#ua>)?oh;LSn1 zC;A0$T{CfW@TTqSS8d(8YIoW}g%w-Z_yq=R+rBl}Z+lv5+CjOQ zM55CiM19j@(mc|}yNJ5b=rn)HgOaPa?+#k?Kd*?bot73C`+rZx4~p8Rxw*N``8Txw z_vRw9Jv$%nXriDWI5jV2&+Z=SbYD8Sa14#YuokTS&g(r2_!Bol@h_IHmY>Vx2K2dfj;8G|G=WFmuKfE8DZKqaK#%zq z2`9Cga<7ZiaGUi>60VR=4<=B$#KRu1*bGIlsK%7__fTeh4|}@#yD*h0Qm2=r(alz} ze#2~4+_vBvTYPGtU~6JF3mN`_X|0MUO)CP4idY~Sb2EZ0Idxwk`usYXtW+jUuMJ|E z9}hAjaA#i=z3XGgBNLwxM;6QpofG-*+9b72FM%5;qdBCF21~}j1?E+g<9Hw+11mtar1Q% zoTg%rD<7_aFgtTdD)`27=Cr}5<;kGXAwq9CKjMefrgOtP;k@RCJqnM7;jmBXuzs=3&w%B1U4Fy)D&~#IsMp+f88_)iF^>d495F_LuDbt$ z*yd#O!Q@1Kb28B5PtFK7crCygTaWX?;-7W<-Zy zxge4_0&SO1$DR8vVD&&LE3x=TLf<}So)YmayefrQiM?R2M@$h&J->%4mA{0C)U80T zI}Fy&bjO$p6G73!1Ly8}09zA33a6G0W6ocf@Dti4@PgLZnr!%uZsto-uTH+9UE(jO z)c*iaowvf?$&Uncx*kyLaSJJ5^#MM}G@{>pDcpS4i)^|02D=My(1c}qnC=>e`!qK5 zx4$&0$ely9aIk}%zr=CT&o4NT8U>OP4*Xk(9;$~}AhrdgYf%=iy{AR(-dhP5w-?aB zXBzz8i>G9VMl*HtIL@#Ae#m}j$8c%;9I9aCj%(ka5*&zH!M^5?7JNV609rMDf{&#g zjGt78l85cgtl}V=gnotanTmMFZ!4?`RcC&S7YH7=N%4Z8cd)?LfE@Xg1QNU%M=YB~ zS8Th7=PifI*f)7}e-{rb`*-0f(VNh;S&`N=3Hm0unbn3I6!@#j(1%gwWYT}_w8LN@ z|M0XF2dm53^&8DtH#(ej-20CyUos@JZ#=lMyAto+6wCGVW^k>=^XWUaT=IVNDVSS$ zfGA}Y)2?}!$+e3q%u??pdF3lZ`%>nzdwRz7G@HncCKnN1jo-|q##wOfzh|J4@)!~t zE<+2I0$m?twp3CC8-un&xy}UJ`<~T`V8H z`YX7;P^Qy72q}FULC({oobP);GcS9?w2!mt3$-{Hc7F?Adzb`9haFHs_(0ejQV)wJ z9Od&2Dq)W69C}1pLff^RdFmK#a_hYg&%RZQnGLh}1B)>nHACpKd6PI_n?*a&pDOIVABL`%%vjD2 zO-Rl@PgY&}LNs5dK~}o~I@;&p&52^DpprynPN-r3o$tbD~2tBT6aR$ZkvN`Ow1TdE2U(YN>!*V-P%-K7udmNpw!SJN$W7#)J;Bd`z7j zCmvgQx~>A%yKF>%wF|&?r5@xhiWam-ctPmvNuaZ$o;2rL(~DhY)a;%#i>K>p$Y*EJ zl~2L`peMrI)KJnznuQe_V*Kr>V7$`g4U#8&AZc}paJ}OT*8D*ZJ#Wv4R=0Tk?ERhu zpm@D%%u+V;@-sZ%62*8;tiZvx7aJcMaPu!xwD-U1wuUAP@tNo-{*14L%tgoO=F+&gjXscmPr;b2v{xA$Sn*^s_ zcjFGp9MTe!O9IWUY;9vG8BsBnJ~<;rOZrC&e@AY|Z-Ix=Yf2>UVp4Q=@jmkMS2q#+ zbPKy%nxS-L8RCmcROQ_T=DMm5A9#gQ{o|M62lm7D3)$q{_XJdloQOuLW0(O8t$VuR zh;Yp&VC!Ed5?A+A?AO8w;`M$iYjQ@L1!rcmqdQzl!8}{i*l>zH2~8$x{-@dMSJ?uX zZ3_RXE^5$yp-AQH4&y(?*WCU0Wcb!Q$#!4x6hV<4LW?hw`3gF8mv}AhjuXLWoj)TMmRyko>m3u=%D!RTw&&o`{+z_3OG?n#T9ym{>tmnb4Ut?zsmu*O zIJXior*svIxGhZ+KPTW;eLb+BWs84h62VV!h#c3N!RoGEBFAJOF>&bx7N4{NFE34l z>o48GC*~MCT5=z!{QV#}bPrj94-v|NXO4!$DXzR zU?zPJ+4rxrG3CA*jp&l#jyDQO*O^wPJ2?q9=os+ot4~03St^Ri^ay6P*5T1v2dTsx z7pin7ln19qV%WuTe4)1rEeML@4>PZUd&mm@C_e@MK^>%ti(%5*RJdPQ1v~ah z@IVng67pSvR{9(B3*+43d3*}*PyB|)V?MIF5)(*BNg}qU1vqkIC5w^H25W~DQhVBl zTt3{6*Vc`OhpBq_<^Y4$hx1v~vKsv8*ADewi*cl*A^e#&5{HE@gDuaeuq&1#FevsB z8bliLx_BsQJ2@I9_d4h$%QTT51Rld3CKkgFrhE|J=$e@ffb|@ zsh-8;zeA(3_;(+A8haC;_d#s3ge4#C#c|zZ?Rue8KiP5U5gPBGG6b=+sE+&`GN2yG zKGsz*`KtHK>TeIxU9cOQoNtktxqD$$zl3n!?09z5&>cJM&tguhBIrG8hD)XM@j`hN zlWkjvdTADDJ>L;UVk_YGA{Rl_+vnt@!AgbLtfDY-+W{zY z1aPozC6gxGKzO$S$sOAV9-B|n2HUFLKZ%L9r#bA7<_Qgo6t!Ajnc2J{RrbqA$k@ zjx8%?&k`>YS5tk~@u-O2wJ%^BAH22o+&e*d?frkGN=k>ExTyk4i?aE;P|n^SoWVy~ z%i>$hPnaty!?Uc$^0zJX*}%b9L~BGhAR|8~GJhxc?hQG*{w zf1!225LQl_JX8xrFw^EE9?k1!GVR&uRO3efyQ9G_zXP6hAb@W9G!X9g*76tlB-)qWx^S45PmO1aKYOv?KOb%LO40lJ zF{l%&Vqk`!QM@Zjhar(#Z7PKwW6n5DF7>(BC z=X;{@)2;EmbIk-A(dj7E%^JximTsZCrO!bBNjvHOdz$xVPbZUP{@~mP-H_a+&bn?L zBNs%M@wqGaLs9Z0s_&r9EAwqw?Z(6WlCK8~_EE#V;aMCWhx6Sdi{W)3@E=7Z!KC#S zD~*uE$`>;DQtk~o64iy9OM}@~r4|^bq61x56>(tRN~r#-AsqebE_lS;!aW7{?9BXN zmNze&J<8Yz8;uR9&#pRLzVi*!a|ZBikcD1}WK8wXhvxwWuv5PQMl`6iGL3oktH6cY z*~Vah$zi1aLhhz$#>ZFxf~`gaq~M0!(7839`W^j?%NISs*NXuww$5dRm6deT>2c(K z?;(hJxr~ptT}>9w`~W2lE9!%TJiy;#5*?EpKo#3h@_@Z+EL6jqx8?^zLhmnr$}Z&lo9hCA~pP~w4ZR#alcSQc`80d!cchkv)yQ1!%V{4&}deftgI;K?e; z-8Kg@%)DTyxf|;c7GmB`efHUzV)l|8wmR#k1>^V>42pY2G`>3X3zstS+o&p# zl(S(zALrra=sI}3_5>ELTPC~_yP7xGRARlzIXLEZc<7%vt0<#<%dAYgTht5;=U3CG zMLT$8;YHXzW(!w$E`}eIU77F6YgAfr7b`#23YYbLWIIJP=*;H?ORaX$W2b+?Kfx#p z`h`R)ThS(3uOF&*Xwy@x1JLGH4vwvw!K8u;q4g3Y7aktK)$zL_D}NYV6iBdjVqwf& zzZ&ILj0krm&>V|K_;bd%{=KO(pLp7bR!oiK_D1gH+qf!t5n7Li*+poz zbTqUoJr_o-oDU;ko=1bc8sWPs_IUAmHQZTmNn}G;Q1SO$V8ZJd@+xr}+f;Op7Uo;R z0M&pZ*=)8|-vviadP2jiGubS+0x+rP@iv6h$x_>wU}S4c|l0x)*qJ<_$uq$ zPFr3D_X@6&CixbUb#N(sd-(};W|m>^PXj0p&7?K)Gw7S@6PTYXz?~<|gl-Y+Dz^3ve*C!0Cx%)>U(qWVym4T8g|$JukU{rFEQhWrSR z0mrMiSat6`rX|(}7f+7GU(aI!zB;0zlp0^+dKI!-vhhkmFc&kb0mEG@8(LOvgS^fl z$h|H6uOLJBWb3gB8Ge|VaN$McVbA0K=%6RJZc!se32PcJh zhBMtQZv}g!YRI0*1A@m=^@91Lef9O?o+Qj8iX=>JvH6skE+{$71(U=6u=O{Sh5yaa zv&}glg5^dFp!vpm9ML$Ow@Xi^owcXz$_M?J{&#BvOM@UzB^uZb5&HG|Y;at49hGDj z6So&NtiY=t`GsQ>Z9A(-vND=_bQB4K9{D`j&+gLCvA9b zcrK0_kfS>;{9)#st2jY!=O5)A1%#-z%1u1 z-o2v++e+K9SgD0K?^S_FO)D^Bo3QweEx&kNKua9!=z;yEr0gh$$eR{w7=Ts_|qmL6)j}Ph5)0)3psqTubQNeaH+p*=8fuX zJ-hU@hOQM#@q6pWLU_X{@UvaYZ`fYpA=Pu3sZc{8VjK-qvsScE%z}LzOc+w;Q6;F9~>aR2C6>10v+%uSu=o!>Z zIR{_0GKMJh@o=a}0y+~*>t(gRkj~ZVY)#K>u({dpUDFjW?4?*qXOMx zS|L2->?O=9HG!4gKZIiEq}U7PVRXMi4E_ClCmE#~0Ea%83ms}KS+AQmyjrw`e3bSg z>m5H4mz1q=A$bi?0RvVxCKLmQu$o=cL>yDNf=|-@hKA46(ZcH*ximtLge}s?AtD(X zt)jW4cO#T4hGSQwHlG_op?5QQEht?64_>_7M;lk=A|3sMYzTY6)N<~EzSKu%s?K1t#c`b1{6mmOiZ8M~E>cn{endi`^uSu0r6;%#U-nD)KJ=>NJQ&h|` zXlV*A2)xT?3!f6v7g`vh@CJfr%He`lX3+B`fzE7uOI8kC5=?Mh!8+{Cu&{6)Y6#Ag z#B)D|qaPZ<P*G5uIzjmBT?Lkp`9MOO zB4OIirc)~oaemV9EKQWYNToC$kq?UN`GUi3JU8ho8oZjy6;%$vHI-1RT5V5X z$L&C+4=cI9;0SJ;>Vef7FUTpEaBSLB0*h`>$AW8CaAfRZVV8BYt;}gHdLV2oo$_1> z&m9^^59gNCPww4lxpO!cS?y*A0xJZ&ms{}3E|*zz!f!}4_{~-{SD~SdG`Z5c93)+n ziI&S6^8M9sra#vXN2)&%ZvQp~EFYC&wV4YHI?D1BuEhmi+qG6%l zSo+|}FdB8+6{eQ}G%tO~8e7ttldV2XCFhBJhmE*g(mu|nUN;0cw>208Y+s)(4#NLAXUrZMcrq4*(`1LdXX#-JuL+< zdV(SBqaoLS@_O#xNTEW+1Hj14-xWG7l~(I)Tj zbi%(dkUlR#|9m<@-#*jBGk*@VZ~Mg{+gple?~~-=zs31a)(g_&_o2mAhvx1T;&O96 z7Oypm|9&-$E6teA&&DXw=qqR0z0o`2`zH~8^!|0C=_NxhwFtqaGJ}|`>EX)~4f*A} zr^sT%D86*>LU_^i40j|M!1zX4P~A8jGn3@8<6b-#gyxdvrP;7nu8QcykAP3{H}Uy>*hkQI zzij@vjJdD)GYRKSKEQX)p93!c zmBPh3gpHTqV8nq_@TmMJmb?|<>5w$aSKH8ItM}1INkQOwritL#L|K{4kfz`HjY~oA2JP|1Qq=ctz9H4D(Gx@b_9{h7V$UfhB zB4|G<&p%EvMxRw9!BWo_#uc6h`vfu6c~#AFcgqVUXV`=4{6)Cvpfv+rj2-W9GRO1N z@O0r(Tp-boa~>Md$O;J_+))d@t7~y{)JVFCc=BYUk4RQ3@v>`TMEBBr+sl%raPRzj z?)$C)Vuj7!i-slbjyG}ZkTRKj>Yuyz8($XThnr3=RI*dkqB{&jEHA9`b(&RMkVRC zFh<3(PHcw#4cK6jiN3RB*rAebY)XhYUL28&sS*Yl93YO#7QyiIjU1c^_lJ@naj-Do zneQJzD5!kbM_v{jhRtJ#xR@GoDE(yZZsk?y-d4TgtRH7Li3p@}>Kqp!GUi{(%nl4ujv|+Oj}U^GX#s zggC?fphet%a2~7ZREB#ChjYCtv*^J$uhHT{3Uz!Iz_%=14_6HAAb)QTD7!^sL-QnV z|5XB8dA>vLtz( zD*~C3$U2le1jBv~^&^``ac_-R&`{Y7(t1<5(KR8ijn6^BuBEWLSQ+-IsdA%-?Wk?^ zj8uG6W^IYOpwyR-!dZJU_HQn9cBR0^{MERqZYD3B=E(h>YGLEsZ+Lr{ukdc{aO~Nh zMRgr+GA-2ud|$E=tv~EWCZ5irHBmhekAW34}rs)GS-mv8QpiD$D5iF z!q4AZsqn`tko{;yE*okJnT9fYS4M!!#O3_{CmHUW;>5jb%7|0w6lOQfTF_gg0jsXL zz(!Yh!J=K|wvJ_b7!uga0;lybgTH#1qtyj@W3IEU3CW~nY#9+5Hw_2t4`PbgA#na9 zYuns55nU}OQrEabG!uOZp`~H`l;<5jKmHokE>*+-l4r92;y*AScU4w!@Cm#gvk?0& zU%{d)&%h@khOWCT1#+wVz@p2NU+MjdK7%E&WC;7Ys7&DlzgJ-8Vi7uZ!c>|jyN2s- zo=RO^T42=m91!aY#}Dp#SQTbYx7_c6?D5mc2$L)>X+Oken~K5HMar-}>joKLDT_7Z zENMlD1nvAbhF)TyNor0G`9?>{md;jT{ zV0V%|#5eE5VG6@}k&dn0I+VKfx|FyWhLy@K`??XdOERhVo! zh^je?=&0<&ew6pH&1;p}l>?S!>Y5Z(YI;gPY^ue`ra!`5kRST?9$5V~g*+@9$)0zA z0o%jx*pdE4utIbL%r)wQpbv4}%}1VQj5X{GojZgDso#l>y;$!H_GPtRzmHtNtNG8?&Am?deHZ=`)wJ)yF69y?-d zO+6N`Lf2Kvxb*5Xvgc(mnb={(F0MPwEZ!a_CIib+JJJJJ)rGV1H#3AC5nAv>zE8Nf zaP5$8sYCj#`$$)g9NT-o5Y8LV$81M?7+dWr4D7y4qD1p(?N4+5q--^1^G*m}NVL$a z>CW7^aXfyVu8$g{DVe|GG#S_+#TUywLZ|N0blJ8z)_C|0d@21x{EqCz-A*&urI+Q9 zUN(~UxKxA5kGb3_Dv=kgTTTt$z7@Fi-J|bpqhQe(8z=~$Dy;f@6y2|X!|Pk0a~fg@ zU(V>_=|_KPL%>GChg21IEw_|rCby9<%PlZs##DipX?V!sSQT>=t~#bN0sPyew1ZtXS2-?lEi;qKKq&=f)#IKq0(KRd~E%P z4^D0)!$JwpPK-gRT|daU`t>#S^dYXv_4-rM%BD2A{% z;Z$$zUBSBUsUX^Z#x^;cLY>JCcrrl)1{>eP0=J9!Z?6)be36eSX68dYosg;z;Rx@Z z8_e}1hwU}{`S^J$w0p)8KGau}h;!YLFLI6)pV~-F6+fZH{n6MKz8+QDB&pUg7p{3( z1XYR&M6|yH(PgHnA)CNVJYNeJ*>b#cJDeYWlL~cHp5hA2Oqg@j24klFWHqaEAuXg; zI48OV$H+{D{SQ=O#^y@0fKA8g_cJ{AHxeQ$*P(=DCEwo` zM4$JT(!f8lxc*uuIz5_AG7U4}W86%xXP!t$PG1TS41DR=LpHEakJRVkB;K<>72X`& zgoDG+K*zX)yyP_n-;^7q@l_nP=_*IRiQib>vCDkjCNVr2q)q?oGhZot8NGSB?u~=Fl5o)VajAQK(txg0KDeVS~FfanapC z2ER$!-kYvYuZXv?s?8q-LSJtPS`mf;ZwHy6;wZTrd5^5^9?}CPZAi_}LwM{!AFj4H z#eef(q3U5%{FIjkZCf;8l9Lfc+Qd<*#c9-Ntr;lzB*Xp-ryxJ10LmRp$c7o(C}Z0} zeU=2m!0}!5$U?3L#x~h4^oD@1zHfbTa=#V}^!&>fx#h9Wqw?q-X$8xQ&#*ab$Ksbz3GO-O3)5J38Fy7FV7p8_ zk^Zm`<~l}$e!Cn$JbOG8PEw)kJxtotgbHTv zL9y*;p=MMGd$3RyJx4c^$S+HveWn8~sI2R|*=fFOMFF13E1 zypv$_i>qY0-8rE`cN!X8^0IT2ZNpbfyXjn`sr1r`J=E*)F=lXWINNx91bKb;lOTD# z0*@VilY`4WKB9W4r<6`Xg+|80M+TwgeP4d9S_U*-S3=ZYZF*~G1;5c5#;Y{vae7jl zR=PaH&5QMUPq%8l@u4|5Eo2m&TDXl)ia20I#&?l!42GY4b zG1Sjrjo+MJ%mP(!($~9glMD%Qa{1W~Hf!7-%pQ1vL3d6G?z<>4OCt&BZJtcj4Afc1 zDHFJNy&fB`IiN;Z9_!2En`d7+>PutSK17t4XE&X-4>&`Kp>L>*>WFv@m{%2aXkkU=D zMSOkv7qY+4guG17piV=&aqP`Df!Y#Hdd>Td;G0MVdYY+3GN=S2~u}tdS!Mca{q( z%#N_)ZB8`&$~?h0Rf_9u>zR34E;Mc~#8nHdJ zTMAhs>U4IJG%}es2+%jfkKn=lG7s2P8Hw@KR8u}b`v``leJv|;P44kSkyYqiAi`qHM^XQ0?(lut8az=thj045 zk$-Ab;D^>m;#K9}XmR!+6J9!i8`WRczg##UOzTIoZ_Dnpa}`I(_?|>qdu#`(9KMVd zy_SWur(>bzjSK{}G_wbDmJ&(ZiRAINEWyL&Qt;o+kAzMT#m^q`?058EICLU{epuXr zR4TyJAOi8;`W z_oBaI!1G{XMSlz_JC?{z=f;7(Pc%_1)+XoPnnBLUHsQbBU#aTZa;UNJgy_mR;#?^Y zUSiMie9<}dI;_h+j`YNNdyiqz^-e~I@QdS=Ji%P^_iT6WLqV>_PMm-DF)1Ev4=a3M zL*K=(q<@tYxLqm2pTBxw%T;-FsA~a3&(XZWay+fB{zRWeT*6C=d*MmMSm-q$U>*mx zG5uW{inQKm>sIvev&zrd<~gGLLZLmmH}VzpTs#PS-aliOejTKGsCE}$o(J>SA3@Rk zv2@+wNdC3Lk^DDf05%Q!kr8R<;XrY>?aqg4uzzwO)!2Rrd(QkI8KUuE9kh~-h&LwI z!YDfZ!3Yq$Qx2W(enjVj4xMUj&)qwV$hwdbkR)EqMx2QvSC3D`b?ravG-6wXcSlV} z!P)~Pf8uhMy_}+h>pTqXejr%+HjrhT&t=YmLZNPeJDi^{%iW0v^Ze;X)Xz`jhX0*` zXYpFRNj{rf)Rj;d#Tt6IX?QsC5y|v7aH>Jy9Jl=AGeXg_F@sy9@GHWl__CY}$M3CzP%i@OeI`h?pPYDJ8LT6Ip#6=KfEh!n2vPE&s(@|L$~17&kQ=eX$OyK5+}>#cB8=qX>MWP&H9S` z$g$2~vZK%oiWS44^dl5ju50DQQ{r*)9W@ERltLzlOb-* zYE-(BgFbo_=!WxUNcBc=la}#tAfpe|`+FF%9zzU%OOTG=A7J%*Q9clw$Glpuu-NTy z$)_VLNOGPPT)tC8OZMIpw4V~^i!NrO?5)N0g8oqK?OpWDTnqgE-X1HRGI_#(ihSan ze!OyAmM$gXAh;_}pFDS<73+?3m!o1BJ{kl%PU>`5niQR>76TL1O_}L)CtCkUikukT z$>#Z9gsjf*2%0ehnUNRZ%jrq{!{H>@++WL-kN&WphbT#>&ALhS(V;+c!5kGW&ULpA!w_PWsGraEMRmC!dcg?+sn zhCi)B(D$ki`mQ%cy(QyOC#N6R_-fPiFPoUek9Zs%@fUZ= z&K}jCV;Xx5A#kbnP`NRlk%#9U3e7&r`V8L@@(>Mrm3#{pT(KRrrtIs7j9KiCv*^r!ZUeLJjJX!wY z5RT8RhsFzG_(SG3j8muVhqE=BowelZKT9FIGaHz4I`NL#g!RG&?CH0~WTaJ=V9xFw zVTkwvb~Rg3Fkz&y{?y<}CU_{yIztw)?g}NcEGe%3hu50?6LV-!-)_aXdhfGK?wdYoleBT0;GVZv@xVzS^l=6zg9%lD0;yNjpr zrPnXQU4Jn;kk>1?{c{`}^(q!qHFDvI*&2S*iQEvhz}oNi?54ggxxVx((daF}rNRlMc$N>@UV9(T4{8gQ zg?T8p!jrAb(PjVbR^(7-2psAotZXNG)ob*E;5XYLAm?$B$jGvH*+Z*MX>JP&H0Ww6(Bn^@R+ zTe#eBfw2jP!Buk&@!Z}ie7If~tNdDVuQ6pGE)It&#*<+B3`0b$f;(pa1UtPGK<&K* zRL+t#aNN?@P!eb9!(>Nyl zV(!3V>aB>zk}L=eU5_`7uR`+AWa@NtDXjgaO1EB3BAe`-VcgrH`7i5qENU>~*E0<9 zRO)d2lW`gn77M`lW-%m$i*Rp;;plPGfq!Vy;9=7vS>WO+^x$U$DtGLNV3WrMc;|8j z8>diOFfg=lhq%Y#!#-n=vkzoM@{R^K5ud14p*zD|Lb zv>CS?ym}0!dyP~08HKalbo97-#UTx-@!x$oRP_if znijB^7-4H35zi-&Riyt}4;%6{^pe3J``|~C4UJEp2?rje;+r=J6~MHXdC>2697nB4w>|Oc5az7zL&;=Es(*C}y6dS3o*8cASts|iNuSPR>9~i2 z!Gs8=cs3Gu-weRie`QoZ=_yM7_(`*5ZbOWU7gaqJ2pW}NaqXPzU~jk*_`dyAy*`e& ziTASVRjyzVT+V`jeaE@>dUW&T7)bR;cCF8lhZ{LlL+k0nK$&=cFHDYUKM8`u(-pkU zp&UHMPKK?+Md-f48?^bwMf4sY365X)(J!kmfTAE6G`6e+lNNt8cR(E1)M>l(L8PE{ z(P6ycw$V1<@;kYx9b2z))R2vTHXhnII-Vikt1d%Kw>Mfhpo8 zyw|mT$Zyhy?UK)kYfu#$bcloU{qy9}im9|Xq7PedYjW{jD*WrUJ)kn%k!BQ{(t7tg zkgb%VQ%&?)?bt+aCpnWHU${iJChj?kHhPNdiZ@-3br|vv1>F2 zT=xDVe^zEN$I2SE&)k)LJMTNBKU;A7;%amjAs{-^k0(5jr!n@MsZ@;*ottr!Oxz)Y zrp?XldYA@ojrhcj7WvRwLo=zRx5UZupJ8On5XZmC#gU!yl&AlK*YT4zvCOb)CX-7r zp(%5m@%#N7u=<7-U7}$}XP(``eh=~b=4!@t(946mS~u|Tt!cdLoPLA#%7-}lLL@ol zW@J~j?L6o-ErUHn9w9XY2Pp4)%6=~#PUFkuxTky65Z|Lu-n>zU+c#b^@02?r@hF?D zSiT+JXPPmWE9;46X9g;bSx(NrSH`;?0fJ8G6l}>8;xc6&RGzvGN5cUeReFY{^p4?v zCqJ>jX*zV{Sy7M)PscmiQP`Z@1bgef=v2Kp(EJnu6PBDHK6S-RuH^$;D{o0}Rc)ai z+J!J>XrKMnt`qLxsxNGPBTbk2$g|aB%ZZr2A+q?@^k``TJV@50{~dmhrwYAE?bt^) zYrCuXW#j!rM#vJv)pww_V;H?{?Eovo1aPz9CAUm{2B~k=$aRGr9NGHz$sIiDtXw{L_QGb8X=vo%&E91_xd{leB)qNuGu5~NoBV$uFP1rz?v!bMqg z*k)x(p?05=;KI8dFZq5#V0l&nTB04shYPGY)w9pi zc?BL0`TC5{)Y7p_mNCOpQ#=v*1Vfsa!EY-hffw^(^G+`q{ptY84w?!xHJ!1j@k7*a!b-ME;<}|7+vU-?84J{(+-pip*q4(V#>sA@^smk3^^xjR=*g z5Gp#-phB64RE7|dWQs&N?t8C~BqeoHQYpufk|wE0r&7;-UEl9-&+`ZDYyY&bz1Q0B z*9zSpwDYu&aBji>*7EW~IzBV1`U1Ty73j&GrkInOn@Qx1_N%S{$5ryb&+ zddXPEf*al)HsoeT_i)eK4OlUYQ6hIto9&m>u>8L*zWgf7|8Mc-MMM~}0~qBphUvF{ z2}|o{jg180qEE z72{5-U1g~KQWnm%b;3w|HfHIT(}vVs9P+M1u>}*jN^ND%Kx6@v{b?gU^W4T+*Xh8K zQ|`FCT9MnkJQeIW7vtEAdg9U%ML*Yzv$vefIb**uoKWS@TY2C$9rcI;?P)V<#`@zl zu&+f>QCx=KdY#xFksx@w(-xDrDRC)N&vEZPvvAb#B>0FH!|K~EpfdXvIh@?4uKi(Im2iHytu-LZ+Ty_IG6`v z);1(j*AKFfq~M$J7;t#I12)UJ;eCh4IK?yrGn)+I!^Ed#$%MnCA}AL^S66_+IgTu} zN#!(X6a4+z1RVbWZuh={<|jU4tcMw<@%%wm!I6CKa3xlOFL6?XF<7o~08^J*tlIdH zdW4@~(%YvK&$-KRSZah`c`!v_7n6m#FXh=`ldnWP1hB$ff>!nw!Gr@>@V4ADoF%>h zZa1$ajeoLfqm46O^nEWl9%@glZf-zp{dmDp7N9{@KLi@}&{5|mc)bNs@}d%U8tB4D zt4{3s*&-My2p}sqP9;xz6RGE}JSMyE4EPMTP$QXWTofe(nrdg!`_U@YYRRT$oxhSYEn}3kxkHiPEL?=E>>g&!7}os;0xx zo+EJLZ7&Sa4KV9mFwdcKE42O`!g_uZZ^@p0Xk;Hry*k7og|ma{%8|Hx@Fkt`M2q9$ zFOcxHCx%6Goa*)SU>~v?s#>FA-0?VuQI@XI&jDVhKKXuNj1jq|40X%1Vf`LI*z;!+ z{WtC*cGqTM(WF$yz-<>*_qziyCPo^2CvxkuAK*??N!qtRs{yv20q`YpKdM;m;PrbxW>w7F0_Vkb2`#nqd`0a!-zfg^CB)U2VXvE(r&>x=uKz z8O2V$c#V2Lb-^(1JbsIQjH!3#Vsq+WoR{uVayz^lk{gV1Tr`UC{bNwfc`2OuqXpw@ zUeJufbV0|Vwa{)8ivrC$I8krFZZBAjt#E;i3N3_R_%qy-rYp$2!a#ssAEOsM1**4A z!l8=C%teo1bknh)jLzd8wC&za7k&%{v+Q-aq1T?K_|E74Q5>N$9X8bGRx)*u8-ZJ9 zj&R$61&t$NAg(k_WBiS%?N%q=Jcj}_+wqS8q_4A#x-O#QDcpSY5d7LJ0?UelUGz2| z@^8MU@2wfqxJn+|4mm^PzuR%oc^mT8JQNb|2>V!@FXI%GWD>7ogwIQ&P-o*xe9`g} zW<76%-qvhp-lro7dvZwj8CkqxDvNp!GY2RzQKz{&7<1(=*C=|x$zM;zxRD+l23%r#XNEX9H;;k|jT%>GJsi~X1m|R3A z-qL{-l{C2D_=(ZdUI|rN7x|@zi58vx1+*xj8J{#P0QIKn zWCk*LQ{yZdXitQ@$+hG(u7^9j786BBTUvkjU;HxmnI;MA!a|8v5TO+>^f|#qVyGYE z{>#Tl@8s~0aWCwRMr>NVm;8922Q%unlXkIeH2Zc2os|nnXm2XEcI45gA>k00)(uk) zgK*b}E~49(fNJ7TNL82z(~B)o9-0SWl7MI4D6;dU%5YcgOSpFLB$m>XaBRj(I;DAl z4tS_R<{CA4GNl}b)$HI3Ok@A`RAGBWMaitB8uBnojPx-Krd*UH^BsjbgP^;(2|r_D zWEp5J-wD#*BYcS=AEr%826k`ybnr_PZi<(4e1murVVDQ*}tDH_trTRQD(4n2CbocGIr6mcK`{@HyAWlw*Ct5%}y zdsdVC+Pju>5(_5b{m+T@ikmpy{|Mb>z6}p*>2U74>A?Os1?Fl4D7J;7>+(eK5F|i| zmnLS*Rzd!vx5OnS6&)hJp*+(MkA(>zvHUQyAM^0AQ#>2ABoC$-oWt#^by%kQ6oUr4 z$i-0^+M!P=^L~suex(wWwGvVBd@Hp0$*~@;EIeAPg+_IT)bMo|ZBqJ4oF(%G3sf@D zi$8&@9jL_Q`dV5e`vzZJO(lOGnz4?T{Ge=_2%PH8$Jq0POIc_}x5(6i$Xh?$Umr=g zY3aj)%U#qZIsxyN=h5ceR66tPriPz6zA#gN9(-<%Vq@%UkYpZ4?>#j%U+@n1?bRgO zk7DqJo+8dFQQ%CYuJM1~@W6c*rF8N4nXKVsOPDC!Gc5|~K~q=KmeY#0w=Dp7MuMHG zfNXz0i)|Qw1v7oD@YG>toZVgv=MLpDM_6$ztjxsARb%K`c%F8xsDUD`K<|=2jf&c$pTGW zs=;Ahx?t~RFP!ZYjee6|>4YsU+PaYye|QGMN6zWkNsyirG3NKf0lz}Bk!Zu;mxFQ692sI8cf@KjwkT*#um4Q3U;B zB)Nj72ef`r7fZ*kW8di;xM8a_8{EAdng#@@msmrl`i_v}ONZgaZ*#KCX#o0cGeGXV z4>l{U_UqC+@8^)Dyh_x*mjW@K{_I;xrG}5EqOme@ z67AjILZtQQarTY5La=%rIwxxL=VTp&GLsxS{b)T=h?ButhVop+SQHuAZ_He6*1!Oq ziC47~@b{n^8?xS(&hkzot=fN+H`>?D>LisE?EJ)(Qwjx$?* z5573HLfNqnFkN?++>DcAI!1cXOjeO?+tEPue#~IwA72+t2vh~lXMcE%h$=XmiCfw{ ziJ)t@)FS#j0)K51MkmzblDp0HlC2^u8$#gfM@jaccQzx#3&6Dn71Tq&M^LbIkbIUq zBs|BW;1OXBUfceHD?eOtaAqOl#U_yOrV4(d(s9&zSA)7)iC{lPi7VK~!Zj;z*!s>E z%pc99hc$Ak_6}EQ{C1gt%l8%f=!{YE=NZfylU^i0yGWaK1{z8iz)h1;X2kCrgl|fQ zxbBm1_R1T+S+Ob}G&@QbF^wSbkr$XqsiW)aoAC5@8kDV+gV<|4SY`K`xLwmFGoLI) z$!>x+U*zD0h6uO*vH=dZ7_-tR6)?0<0*iO2gNE=eqR(E2Xk&LIhHo)$p)dX1yqUN4 zPl|Aj8dheU0aw#VJki?&o5LfS`=52-c`OeV7AxT3!ZO_1ZcKKFzhdqesbFi`2XdBt zqUmjs^oEK7*bV4mXZvXp!}!@?kwmVF6_ zI=ZRa!K-MP7lNu9<&a~%4}8WH5PNS?=DB!CFglMrfr7^PE-$=99?)E14!W7-;Qu)70> zy~&0~S65E_%uH~<(1bmV3GBA~CiplM434Q9_Nm~tqe}I@C5n=md~VNFYWrPTcu)H*V>TC%DI|ouowX ZfZ=Mh1|>Td{$DR5C^*RHw2zO`{{V_xvOE9) literal 0 HcmV?d00001 diff --git a/behaviors/custom/Walk/walk_R4.pkl b/behaviors/custom/Walk/walk_R4.pkl new file mode 100644 index 0000000000000000000000000000000000000000..2d86dbc27c9fd65f19e40603439d351e6510d8ca GIT binary patch literal 20820 zcmX_{c{EpF)c*|`lFHObghHeYQQz`8`=FG}jZ#FVR4Nr|Qs$wEiYPK?NJxsp=j?+J z(Oi-S8YC4B8kFkid7ia?cinaWx#zCC*1qTLyZ8J3I)TFyter&u_i&BZ89JlKZ1>yd zvv0=w9lJbdZ1eN>-R!Yzm&d;NXxX*9JlF5ozT0<~-+JHplkw3K|2r@~T59`-{~K~L z-bN%W-cod3d_??`cpV#28yXhxB@r#L!FQjJ=l?t+wsBT`ROJ8N6^|A*i?_G8U;Ji1L5&r@*P06EsF#)ch(ww!)Gy{ zQUB4;zb4X?2~%{WNLPd#-XfoB0RW{2;|Exd&ZqY5e7t$6o+;!8(MJ|>?Ee(jzic)2Nv&Y~eWP%noDVzm)WCZ%Czl^TvJ z9tl%5cJux>jLeG~z!w*t*rWK30xkK+!hF%0f|`B#G%hTXvK!iTdl@4BX8|cURAJiw z1Pn7-i`6ZiY%jB;idNDXE1`)o#bIcfIFtLgdr+4tv*<%@8!+}hjS2O`dG6viL7vw~ z(lBf{_AhvhJDp?5{(@5cd?F8Y*WV`h!Z|u#`G|^_+=gD0z||BL`06|1q*`G>ux(le zJMH%#LvI(rg)Lq%ZrXD0(YqMG)VW{+q=IV0JXYD8$p@cgk=5Q|P`W+@EPftB@fk_* zE!vp>xi=0P{9l6RcVA|Zjc_V-0RP6+!>!_P;N_M=HEe7_!`y)=$SRSTVl`Nv-AxoT zx5D(MG5m>r0Mv{9<`;+eU~$T3n13XXtrgMa4-OT>!k{pI#;1hunSCBKXPkk%F{02X zd=DZedod?jj`}!HrccMU!|U_~{21Q}A7U@@uj4=CQn%YIYwJENdpd>wmUUu`*GU2u^SGWb-*9dem0@R5f3K*hNcO(h2o*#;hS+DJkpTF=4N$1$~l1L zv?{}+_7d_W6Y-tSAgkPSkioWo5~2MYmLGZsrWiVl+W@v`r*&gD3F(~e|Q{#MZL z8<%)e$O*R5wFR$55&HMyL2mfbkvE6RQcVRp>gW9kzAg}@UK-a~{DKOa+Ed79yi()y zzpuf^dHztMslt|=Ji%wFW|0?vZwm9>=3+=vIx%lvMZYBW64?S%jD7!wERCIj4QrmU z)9oW@@%Rq5SY!}BI{m_gk-0?uQUc7&vWJn$H;8qoF1}(8VEHK-I{rkVnx>M#v*s}_ zDL)6Q%gR9Mu@+Cj&8Vn0g-SU2lb{QYg0?XOqJc`!cr!o! zGK;zV-9uV8-X;fAzVR0-Cgg1BZh@i88t4kWi3e=^VZby4Ma1HTJ8DhYKfC#4et;?= zInub!i3w!>s_^D>F+}&?e_(vSHmun( z$$m}^4Vr2GdKDf~qCwsaOk;mu>#~#F8t*)~KxOyc2Wi{C03wt4Z?ir0h}U^&IAjW2 zlapa%a@jolG9Tf&qTy8JdJ{?ihTLRKDm@fAm+ysq2(Wge_pOy6IWUk~iA?3Yeq^Fr zyB3bKx=-Z1PQdu*lzo1C7H{u#0$tfpEP2~xzU02OaEzomAGv@WVsHq= z+e`E7YDVN`c{bVvRbhseIoa#;2dtAmKvT04-+NoVYSOsP*it6Wo`}Uk)3($2>3Aj< zZz>}9?e%b>jX169eJ*fF+XXkz@8aKY>QFbWB6|DHFZ7aF5A{b~uxnN@hChiTVIJQF zI^Serqgps|m**rR2;xV7|l z7_+z&%nfY#T#q$uZ{8t3zWOZ4$SyH2)}V$hhu@#(^IW@USi8@PI{kYMR(JX^ z`9(6B*}9mTZn%rzss})j`WA*SZ=a|5(E`Z4aK3fU0H_Sk*;2mZGZ$QqKhC5}wCFt5w|IYtCZ(HVh2h^ zTxT?MUdYGhIP#uxFVOU%9<7}#$>sLt2#Q>*_~<)D*jaTQ!_S-%2wl2pMd(s$`y-UM zO*iMSEY@L)@GJ~_FrC(WTtPDG2YuZW4n-IGA>+C#&aWQ^{!F(2C*BV0hmFJVx9W_+CB^u7TZ5MtOIoes3W3)JX7t=}7YSKsHm; zlo4v3G=zmBSDBr53=3W<4XfhXpw!?f{FoL>#d-|+3>7ozKK_&)lX}299_sM#cWdBr zbveFndFO!-QP`-88}4`US$dL~(w>8#tpD+mE@#nDwU1?M4PZ(0Wvpyl&FTX5P})%yN7PNA zTi4vc*Z)qS-^a7S8@E7S(n)yfvme~ES}=1_J{qJBpSSWrx8SvCC0YK>N{IK@5(%|+ zSTA49w8y%^_sD7juG4wlhYK*f|Z*@q;+U*?DNBtIm%}jp4*`6%YGO*=y6= zrvn`)YI(wQYl^>nw>S|0sd|Zi1 z7skUtSOz{fG7{*e6yYzXLxLQB!>AGmu&Npb>-w&c$15j+>+l%hn+jR?gAl>}aXPR> zcRsqWI*nK7%YnFTBJ87|;i<$iL0n!1T%D$XFHc>dMte9Z9aJ!%(yWXona@~;8sKW3 z7wp?HX)}d6;^sYWIn?9dZL-R90!@Csm%1cmz`h;jwDjg$+}?B=!fs5Ytt%U0$=pQF zy9&9@+HG*FuN$sbFXu~UEQWhumP23QV%ol|m5+Qc$_;*r^W&Lm!u46E)bu}V@UYi_ zZ0T`4N$o73HW*4vWX{q9wbwytMDVD&4U{+h5G-|30>1@PbdJO@zSOZ26s$X0*4`^D z(jf$APuUD-@81)?47*Jxer_Qv9{n(bH*&1@!7ZlC996 zCTd?;`%pgF>ve-|?a0UP^9NALw+^~452s5zFEF2NmHd(SOOn65j+h@<#7`!U=WBA7 zfU9I3RES4VljkqUw)bOT<|!v0rap|`AF*HfGhWCJE_LI%(V|$Gw3@~RXn<$-2zq2# zT$R4QKD1?3!Nf0JEKcj0P&>C;7_UBxT@c+%j(l(wOt~vXb_D5@S(#RBr@I&td!f%1 zRWn(kjte`Tj6}yx8YRUv$cSZUg@qCqgqy1-Qk^c@YTYgeQg-MrcYRci$*O0n3{AV3 zbDAj|Se7kF-1n5;ldJ+qgB|?U5g#h`^am-d8>*`>2H{D4aVmEo0T@TkgwtDY zGuC8_v;U@$#tRj^{&NP_2X4VUzlSg<{Vi+{Iw+*y&B*uO@%+r?Y$yu5U3q%&6x(1W zLk1lVLxYA98|=^mFUd)`JxzyXJ(7Y=m%E6Gt-V>($ZVL>(g9P=kD*U-9{i3^MAO?w zAagp2&it>I3$(YA+(mwnnD~WjkNqHgE4~8l^5yYRNH;s}c8gp+lSbZYOF?aJn_%Ma zCZUhB5&r$D35{dZ*oNw5!b9?h$g-zKm@PQ~=WDJ&PiF{_^@(_S@EfrlPN8jG8=lYh zhlM!`bmyv2$Oy2;MfV#)GA0lnD%;Unqt38|C%J+o1v}LF&z4WyUch7ni`fXpAlR@} z0mlo^k)Zhk=rtKTG(Sg?MZU{~Zi-7tm5wnRyCX+(E^Ab)7HRG(nQ}tq(Pu zyQx@sTUk<|^(llbtgaQ(m&)wijb~Lae;i{<3T9-HWiY8JGr^@_=km3$eQC(0M03B! zO|0dqq);8AS;a*c(&@03Zhm`^iX{Hz%QISOrimyWSh{Y%bw^_dgc6H2x zZ;wn#na%*btL{X>g|Rec)&~CiZUiRk#|iWbyJ4DkCs@t!fRICOaCY5HdcNJ9?)fr? zci)epYg-M`ch5FFI8K|taSos@Z9_fD^y~22z@Noujbrm|9eMVfV$8Xh0m|LOX^X-s zu5}RXa zuv>QkHbghD^9coTy4e}{MLqs}+A~ud`+;HY2i)QmCOAFH8}iNn!7cU(mRav7 z+Uzn;)hNN7iOcZp+tcjH$ZcH1aGFq7Hkgk5JBQUfDZ=x4S%S5xvLF>D0%sX~N})9KD(;w$f@RAhL2Jf0@>Nfd9~Gpt$L_r_ zZ?qL}d2^iAo5+FX#R7rWz7@Ft_e&gIwv4a)K1XQg(SchHD+P7-iE#FPB-h@5PMB3A zLf%-25zV&_ct7t5d1FF^%~&;@TCdf@omR4FTRe|7ZyZf*WzDfxFB1yV z#c7z~dXnF7OANPd0u3_*w&i<0dDr{Aa%*}iX}x$|IO^~-Fy7rkuB=Uf3(9NB{5&n; zlH_RiplLPhml*+@rNZE9mKI1)mf&}n^@4Kj1CkqF3VS|lktO$Z@sDK?RC^ekm4uF_ zE+_T)8kWkuSgO!W(7c5_>e03b8!2Qjrs1Gz*7q9 zAVq%`3ZHDMUY=#@OYZjj*<#vjocqbWmuUDi`#D{8K=_z!ne*&GgFA8tWR)%IJ zE1F5W$&$u1;4mzZN3PGsM(HXDOl%cgl3d6O6ym70uNqa+Pvw`?^#d#Tg{`mI_rw2z!NZ~2a_eG?12I}PmDUSvybmU4a1 zN8tHT0M)A`xY|0-OWNgVU5(Kr_IPDu~u9qDgpI&-c)9yIK46?)ob!87^-m@`lqku zGwTbe);hprwLPR=U5?LIo=+9KQpqmwD`@K4PV9uXXeqgwZyD;9pZA|3662jo;N*pH zq@n}=D6M1b)|(CGYj-mH-9ACD=QW%tkOR{%TX9>sCzc#91=V{=?C`HuaP#XsGZp;= zIGvn`W=kw+;F?#&Sw0UhOy5KH)lQ@L7bo+*Hd~?AJeBWj4ukx&!KkP@7415XqC@c$ z=BaZ6J6=@6<_i<(Y|#l+T)Yobrd`CTkeJ0mk#mgaf48_N1qL!7!Ro`Z^`BhBe}=9eil}si5ZSdfQ^zxPmlGulu5yd z!gQwbPLHik*^Mw|FY}3RB(rC{BYq-6s2y)0uo97mu%{#W?*bXrvMSpO0(EZD(Gt`^_D_bJ;Ueir5% zEe980eW6aJHDhNqsmnrl^s*YsRf{>Y|Go(SB~GQcbx**cVib>xXvLXzMDCQ}s&m?+ph=BV&@nF>`sVoSx29Yyg!_j&EVbn5v~12_343ijX^!O=JJcwgfX z;`irN-sl&-``=mG@n((C|3eop@E4-lgGqdkvl{(y`42uUxKDO{E#}EDtgt<}j>cJy zp!ePk>4{!vu70G_2)f<2iI!km3lR7x|G z_xHZW3BRU;wSEe2x@o~io4;eX7mDHXsBj!M_=@*0j{~X4YpHACEdHs`l-OT60$X(} zvBKyRi>sK7q6#Hg5oIsjIeY^3n{3MemITv5n?x=j5JF9Br_lS~s@aAG>agqIVXV>) zf?U7RFj=6_V{HRrM_mvf{^B;Anq>!rV-l&y=0aX-G={#7Ql%NW5Aap(blzOO7U<)- zAa&*z4$Q0v>5ZpB)mQ}W?;OArIx9%(yHzM!AVPmcyMRLph38*RqOrzN;&Xlnlc4|D zpKo38N2UPR{t1Az8ZlTc*eLi|Fo(~XoeAqYV(5|RV{q4AoA18efhGrfvGV1BpiXWK z-0MmQx5*Fq<@$Q` z?Ss=`b?7Wi`{qDTHu!J}-H+(MD~XmUcw@V&HQp<_#(vB&!Nm{7*_th8L|l3#=1cu# zvlobg_n)_DmYD!cCsh*3r5^BbnKhc#=2t1&ht0bglSRCi2V=Ru8*DYJV0xWLVSb7^ z9^0dUorXmu%JDUG+L?hZSI6Slw$u2zdjZbe7R#n?_z$Psm=b-}0rI~223zNJhN%2i zAyFfaQXOw4^C`on&Bc3iu;DD?kFq=LLH<@&-dT$NUp2YS(NHwddW|o>kHLL^f5Wng znsoJ4X}oi#k{p%tBWG{Uh2{bigubOXQb&VdzcGqlRGP|;r5Ms+`(lVM8%e)BmZJri z#*-|mdRW}A$*;^C3p<==63>#y5cSX*HYE?|sd6vK$}l@RQCJGQH{|n$Q}P*k@gKww zmxQaCQ?c-I7Bjr*%7kNQGW8ion0E0JIlapj_k~Yp%XX{7m21B2ZJr07yuN^RI60De z`WCG1`Y|{>ZIE?#O$1B5Jb~<^J+yqNTV#O`okiJR3J%dCmSQ{$kRvYQRsYud2aH)^5jh%G*zQ$*7D4EY7ey})s8I9U)J zg|nvEa^r3}2$#NtMf)DZu5bf9*l!FSY9~?Evz~c~OynB9d7!uAF!MihOz1anGmR_gyqkapLL*(`g)mb_epVG+utUn_$h4Dtl2 zx={1C2PW|8Bh<~S-mJu>wNYI7^f$S->J3T!-Hxe`e&La%8*p=@0#DvAL$5lWVGlgj z`Irt7;`C)K&44r9=3g;X6qJ$m{iXcFk_Fs5;VwTc=K!_^7r5$ff7sk?$TAbv_=eLv zXw_#~I`v$VK+S9+{9d|>zbSqN&cdm@)1VkGnaa@hx>I;V{Ww0|;sI7FJg2%M*0|A0 zjP^E-rap3_)J{~2Z)yF2%9~7JkPkzXTz!@}YBi>_yuq|=| z3G<7ua*mwC5|5ftqLn~wpWh&TD>H&peqgoKd5oFfFrECd=goFZW06?om^F{%(!yuM!HkGfzR*I!W zS@?dY2#&UwUV$HYu{niZmbM_ zb?pZ3x|&&K?Oe&8uk``u-oq-M`?5m~|@{-Bew1weTDq_(cQ@ z*+x(xAxzBbDBk|$fvJi+;F@GP;FM4vk`xQck23LcWD?B3Sjep8mvEaq(o|)QE7vvN zz!R5Lu)|%Y?4F~K+1B@~c}bx@*RxOLGL^y9`(Phr57pV}s%h}())5k4eF=7N3+2j7 z73tlw*WBtvd)3u`Eh><#ATK{=GmoMEXaCjDkYvg5x?L6V{c6kdvXuDgK?7)M=_L+1 ztq`;>9j;mnXrZen%}o-6t|=Z&`|cWQTly7kWamP1@@X=Cfe^b4J_)@mN^y0>@}ad6 z0`EnB2@`DU*oyjfKuZ>&$*NUQHBOIp{wPGuCZtzVj(?TRVL7fnC~5HtL^mD7(a8Z@|WIcoFo(EB)_b;?Ir4M;SJMppe9`G{`f$DvPtIhh&7uJse!-ZZvpjJrs z-uq1MW(m+*Zaf-h)MM_Dr=dJclg21t1Br53zTwJ6UZ%dACi+h$y!Stt|MWf&vpf!m z&(_co_7gHy-w)L(1oVd(gDb81bkm+rJQa2ascs?IZodE`2Ay#9f(wdG_9dR1w5ftd z5?<0hhnb@#=*ea^O4TGtoYNf0YB>klFaHySl=KOO4Vf5}+YBLTDv*QpTPK z;omy`w$GN{lAnW0${|#?aV4FSkwkU2f5Xe!=eWtK3y`F`4;L2v6`q;q$x}b?fWJyl z%zoxf!iH7f*tf!dyjOTs=<~^s>h~nW9gjZ23ZW*7`O9JWvwrq%$0vB5XN_G;4wF%r z)6s0yG)&sA29YWb%;4Ri;7!;Gs`fVl+vQrAiNOX;RGEiMCC{U--FUvXOrKTEI8FDC zZ6cB7iliaqCRBtK<2ubWc=qx%D(*@_ZWzhbvUUmA7EDH2r$%l|GH6=!Q_N8NNtB|r z$nTT|(5$|I6mE#crG=;1IP-7h;~QktBQ^Mb_w|@2H3F{LMxkPt4cgt>!j2rc4-;0E zg3J01e7Qvo&&x66KhGDiL4#E&_hu&iSzAfoYKidy_jgrIDkD+vc`Q9uwUs|9*}~3V zmxJB&%h@jRG5ooFAnSh;%c900!`Xg9Gs`@3CZkYreWRA3zjbF-%&S&5$LI{{Tr`{< z6$G+4b!oP|Z@RE}Y=$eD8QAo z$p<%owAh``u3tGv4~_f)r2)tI{5)4`^4F6tu52QfnrAWRdle{4SU^P+K#M{n@zv`C zj}mPt`V!AozUia0#TbyzKgrL(F+@x8DX63Ho{rwFik6{eylV0dj8%3Ai4SVj>0%~) zv7HKe-EW0&%AR3q@<*IlCXZHSK76?6BK~Fla5%nT9%x`6(m zdz1p1@<{|2T8!kjuZ>vn{Y0v6cY%jLUCuvvI6{xZFF4S(nAW?7(bV!`kTK(-pd)+_ z&scbt9~AbWtLI0!J24tWY)UcOqlf1nT8n3DB82UVECL`+S?h2{F_cUc9E@4RPs-?hPo zPjAVNQX@fmY&z|73nZgOJYa*UJKmW14Lr};&~q9&%mrld!m9_Mvp-YN(sMw#ojY#S2)uhA186WKL#Z!M;Aw4~*%HfwL z+#0?NGA8Td)rQKZd`FCCLj=jy)1Jra9CVJOeWZTmaK5a=_jWmmaRd@6|*)uC*$B)|J& z4QSguVe9Ry$ey1!$;LT(;4FxSw8VAnP*E!TV)U3rP9FmYgH|)&HIn4-r#nQm`;? z`=^e9M;C+OT75U`I3$e%w*Zi~7{l`HkAvnTJL$%*-?&+LfxdsXkP0I%vD%Lw-0r+CToRlm zuTYU@K7Pv1mhJ%u`(spA@jiG@Kg@*=&eUXLBenA}C)Y=egG(1phP;Jvka(`bm6XNt zNV^JOsJ|OjHkeUQsX7)Fa0nOZCb5~T2dc#VD#0=~0Pl|i?ra*4npR6ev_t~yZ9VAX z`)WMaWecv}*vYO8G&7Skzj6J)vp7AbftpsEakh9izoeQ*dM5P>79>u`5Bth#)o>{) zAzVR~4!H{amMuW|u0#*#OokEOqtK(S05>T9Bdd5Gjjmm2W^pFs_* zH?q_sD_*?t3_X5)6S=tHFj#z4hifJ0i2qOy`DZ33ShcN>jj)=76_%}}=1&9sI+B6j}wA4VF5 zu@K9T^8!3a(F5J`oUbY!enX z2-$0?%kcF1O)&73$No4+Qt39H{|SFbmr9BA&=NmBIZuyI`j!h3C0X?S{3B$#(O#M< zdJ8**kMK|FLb^QhHx+StO=^S8S;Ni&fuQsR-$Uo1&D?J6YpfverXK;jqzHa&h^IX3 z63U+ID$}mV()7djJ4C@W6DABHFa_@w(EE)m{a$R3hHpf`vuyk=MoP9N(EbcqWbhhjyy9_MZMy>axVBQQ~dCdp+<($SRyEo!&O*@DP z(xY<&|KmNi6j#hMhU_#|Z0|1zNl6E&c1}YV_ZqYD7Gm?ZG%CV@!{ed*%4f82egk9d z_tO2Nf^p>Cr7)r@h+TlQP;ft&g_m~=XFPe27b6e`q9$72wI;EzLa@6okrD<;BQC%qzGW-stNljF+YOWB6?kwVGy9=xP!G)ua@naSVX z1!bF$@&ip->~sG{@>wDc?S;V@y(I{GPyQkhoFg>(ryyuEabTYRCZXx-S;Sg*r6AHl ztja;VnvIj*Ch%PIj>Qc1wL9(S;7BB{X-4uJ#~$gXl=tChf*+(JM-EPSHZ9LEwowB;hwTfiHuJU6AbZiuCrb+@z08M zt#CgTYdnuu14S^`d@^^_38uYUy7>m_Okm%KSjdtEw7B9ktooD(;gJ$>(V_}Ue~bVl zsTI&YOiDG0xB>A}S09;ZLA)`8ss|`6sCg zxLut|2lx8WlTPidM@xg8wMqpKbx+vh?$1>823{D)W< z5q}3XwRH!-D-?U*G_sLnmqUf&1C-c!h#fQ!WI0_w@tIC3ge}W~1M+kD>^T>y)0$yC z-0dd`Uv?OeZj8fMBfQOwr{86RX>BZj`+4rCxrA>hDuKA$n)F(=Jk~U86Yr0QxXbt$ zlyUe+JVT2q`4EE!wtZ-SS%&o)ltGPg6-E1DIN$e}U`MHRP}NI_b*wjHZm7Icr3mWXyd)bqp8d+ z4R+)B2ApoY1GB&1A#;D7MDGtn+~4MWZocLd%?w>d(saFRpI^UUZ{#nw+W4uyQaRWQ&0 z9+?qm0Ou?cP-u4qd8|AJ$sksA-JiVvL0J7=V``HuPcPlSj`mYRiAmf~ zZuMT0Pki*Q;$8L7_qZN{O^e54_Hqxp(C-L%*gatt21Bg8?HJ+cvuDh%TrbCIOU|3+ z4wDyLKPZDP+Uk6St1iDhNronwm_br|C^&^VljQbBxE8pZMhutcR@2AvC3nNgM59ie z`eq!pUw(tAQ7Ibfla5aeS|FwBBMo_1gBJT5z<=-@^aiKV){ElQ&XSS5+y^9ix)Xdo zVS&5LBgSmO7XN(ve@x4 zo3C`8TwXd)U~8&hwXx0*eNPPV3jewE#3m=W+&8>>^;)|qKbSH zKY{rs%em20b2=;GJUFclqDkAV_+HL&yGIvZj);Q3#@SS6!hO*FJeN=Tu1~iXq|;=@ zX!KryG@$bemAbCM_e~1n6Hn&yB~|A^jU)B{9f@%hehDX^P~|fvonc+949@*oMsJoc z$L`XV7-Kb_u57-{_nx>;IoBdv^fRk&&Rz$pM|xnk$b6QQwnZ4TcpA=Mf0`(osj@&H z4czW3&CU(!3B@T^Xni#RE{d&(FTU~2z*`ZYT03Hi3FS|+(=gB`1AH!Y;z;e$H21ST z{nIxG$9YNc#I6~5Eb|n`?{y4oJ%l*4iSzPMTP{9xy->5}8va`NgvEYJryrvZ z@uFUJGQW2ikDU>VNo~2fWu`m#R+z!RnV7?U%Xj!CDI9_gp3;vmUVxvN8rPonf=hKo z(tp9P(aIr`jFMNzF>c8)zpRRk&Mbye5tFHFtR!jml!K7~k4&vHwN18g}D_joe zWXft=diH=4Wj11hx$hU!8P~&z!6#Ly6<$PW=|=ZbN788DNWKLKVJmfmU43!%gt-mh z_sEQnle3DL!Y!VxF`xw3G+|7Q-6A}&k0(o&BqvzTY|-lJV=DW zCf-qHZa!ULVrJu{#}-GZ!=BhbAa`f`RhaXGY=)@t13Mtee`O)QS6{y0JM zw)paOPD32t9>&Ap%MP!3WMh)vu;e^n|_L+=MT=vE3&I~u{f zWG;gw>fwmXYXGQvHOoh z-Dd}Sz99nj+*N3~)<~FHm_2VnbhRM;OcgFs>V}0SLQw9#MI?kaXjFa#9RtS+I?Bsf zU~LK+{CZVjwfiR24OgMj#-`}oGLbh=xJ_0TZ{b%+I38R)xZ=pQHookh*S2jI5O|?cp1(S=% zvF-UGbXn!gE4xSW|`>BR19CXkaR0DVbABJTbO8g!lD zmY5>0KD&;QwKYVp=MPDIa)f7AU@?5_+lj)w1DJkZNWQ4ok&4RIz)@V;0}T6KhCUrQ_ed-xQ~S2mMLV-KO!*j|=@XF6T!F@xQn zYd}suW~^vND|;}5AiG(_<|ch5&9BpFxX2|EU1Kn`?Q|k0hs>eF&upM~?gSE_juJuB zWMe*DrW*u0VWdTME%*j405YlpyOXp)qiR0pI?F+9;A;9vb`(l>wF{pAyo7~KkH92Q z5#=o!pt&;zTyBZbNn46QcbNqmoN9qd7d_~U*3EQ6^Dk~3(IHrUC7x)s%^&h{=b(SE z2#?>GPVMw3)0FIyeD~Bx;8*WYZjKCRrkTHl6Ub9y{qg|aIc^ab-BO4NC*!!ntyp;M zo-S}|c?ThGzmc5_N{yGLCpVm>Vu!o zI?K|1&$NuLz=XmzWZ%OfUn=esSu|!B-qa|=jW_0?;_`1K9gpzk>XXRSp2Mi~QVOHY zx?r7H4NZA-9DkYZM%}MkXfo}yAm)TG$&h?R>?3b*Qg(~??An1^We-tfe2j40vo=_y zXU$$sIm*p+emy))L}GiZ7nT+5b%xM85SUyKFuZ=jH*XUMeL2 zeqLsd#7G^m(Vi|Iv_s@!gC+ zb*?9u)U&CAm@WO)(+gW2ht^8yRTOpi!!>g}xJkYi{>G`pVRvP+qg7h@Qzg$TvSQCwsmX3(I`msjlx$JS^XUnLGdDCo&8+F8xL%qwR>+ zm^^kki$dec`$D5xanQKAa0%}z?5?K) z{Wn~kk9{mo4$b~eM&7thmV4R>gBmDo(H@4es*j*jCx!Px26M?-2=(0sQ2O2u{{4JG z-nXyf+m5#~7jFev{5cz4p18vI&l<2e+@1W9dO#}m#Ytpw97cbSfE=F!+?nTytLM7n z{C`5oIQT=*HRBs@X*mL7T7-)DOYuwZmEg1SR2nzRjP6Z5M|T_ypzdOg?#5?T2pH5uOF+JzStPPy~Nf+=RykzgTp8EE|^{hs%Gg!L;%ftm4*qaJV@fE>>5tMag?m zUVb|I7fpg~23qv}mZo`g$~)0(oISg{`UFHSNE_1Vw88s@D&Ud|Q1-K=mup^;(*BQR z>Vq`a`TyFu^S_$6e-Gf%Aks)tB84VHN1(^06@|72r8F1nM3DxR z3WfH1e^PyG%aEZ&CG$z6p<}8zxBIyFw|oDE^}~9+hu8C6o@PGdZ;zVKBUiYvQ4#mw zL_R@c-}XSCbkDK71^-veD~!>3#W)Ldmj?p+ai1YxH*y94_m|;|Q4&cte*kX-A7PU+ z|1G|Pt@;02d<8KvTNL2Os;lfp zd3$xS;0LqHY~c&P7}4I#df<7z7!q88=)_IJ#7XvaX0;`E%*F?0qE4e`sxzH9bO9Xr zhuPH-0sGlsMp$1q-COn(O-i1^bBX;RH#rH8*X+Vl+l{nm&Rq7!a|!#s>j{~eqX`F| z7IX5aX2F4#r^$@V%5=#TG2|p;@vG=I4qF&;k8*UV+_X5h+1DN2Lt^+=UQJAVnj!3Q zS46{UYpML=FQ{1;i5FZNNON%#gg##k*S{vg>%wBlX5}E96p6o1C79isH6GOijDTq8 zz=LL2X4UFK^Gf94uk@K5-Lo3b3_iq@H-(JJGqWKNE7X`)5B*x%#I3RvEXLdz+g7hdl6Y< za|N_#A7qDGK9bI)eWb#>9|xLz@TY4K|6Q-fSpEd@aTo=MvU0PGjvN%U*7F|J~KPVx2(TAVS3+muqrGwn>nxZ&^Me2>T7a_IprqsyRJVNXI8cffAF zMpAZP4`ZYaNXg4+2zM^yz1ixF;gaKMpQaLiKOg`Lo144<#j(o@Bdd6Yg1O*W{Dnm~ zkHEXtT0}kk8ocd`g60P&S%JJ0a}rqMz5Ys&te(!nk}A+|A7xyVA6<3i1#4W!i1jH4 z+WfJO%?zJ_fx%j6^Jy-LJ${kX)XL@C>Q>|E%ZYgQ;|2V6q>aRiPQyg^bJ#Fr25o$` zg5REf94d2{a8(1UbW*n=+WVxkrKUmPWtC!LIxdC{o3E#pRF`VjwqX9}NMf2JLu31c zvDJSKsc(6P3(lOUN0yABv2!U)h*QU=XV#d1$P-0I9yri=iyVEJg1sNkvexO7X_X)k zo7Nu2X?|7wc@eVU*}4set*)|1-()b!YdiE7Z6p8i#Nbwt8N53aLpEm2!H&5DY+a>4 zG)El6h?dz@OW{5^nZ6|fS`whDzaB&GycZ_N6jg68k%hM%s`$(y2!2Q@qv^dUT;FdC zI8_hw%NOFs3Tf`Xx+1SctP34IlZD}5RA~BeDr#;iC!4|?Fwliki>3P^Nlg`)p8?7I zsX-pc+k_q)!A8rkC}yopgX&hmnQB#3@4km@C^%i zD3!E}waW_F*WD%f(`F(Xe90jPeM4}dnWEOYPV`-nhVo^~oRTP*B)FAh`icDNO>hK_ zKa|6j@;1DmGMSphSVRBMd=i|u3=Ph#;vcn1ByUvj!FH=S_%_l8Se^ii5>q+X%tGu* zUW*&79^kD#B4K4{3i99963Ov@@z1`~;n&L3P z?G4NJ@5e~3J~Zae!> z2GeXRK|enjm-6%A`u>GDzom;cyx0Z(&$jaQEemnBZ9Jw)SzzCa%Wzx3k@7FEnC#kh z{L(E?uzA1{>lTREpoScO=Zk$~tAR*rd3dq8)lO*|95{(nm$bu+<&{{d5z2cSTn<@s z1!Q(`2Ra;1re4z*b5#wcz^f3zT(JTEypUliJr@n`S%d6aR0K$U`wkjXDtKz0I}VRa zfz759xEn>s@rLjj_WRwZzoSI_C0?prD3c({j&Gpn*gopmmzs_nxTJEdIdln=M@;axS`NgPs-WD`RFiUYqZt5IoEEJ(B?Bz*PZe|h|tJ#rc)-RtMV2J>2G z>nY3qY+VT30_XA^O8j86S~OTScJg-Fs^jb;IYPFjvD(oc!c&iOaPdAR8WbtQ@AvJ& z@)ieTw=0NxDL3W<9r6hnv7ZP zkM4kb6Y#>bdMMPZg3U!&;Aw?2Tye=IPt#@Jd14J&^P&z!&9CsQS6^-rk zba^Ni^_jCAmB#nulISUc0L{Obft$M>U@-@;e7AsieHJeLwV8Vh2RM;^r!d@L0(+BI ziDG$W7QnE#2Ao>g5Wc5lZ#|X3j!UZGpJ{;x$2H!ZpVZ|W|xF}>4 zjy#wGj>1p4$s~_tUzmV(h0zf6qmX?t4}_>8Yx=wW1^8~Vf;EE|ab@}%X#Ww0ZmvpH z?Lr&>nZaV#M0p?_{tBzt+fbSHz91hGjfOr?*r0nW?sZBd-dS$2+NcJGGRBIlNLRk_ z)D65oUJ9?rr-NK+4X~BsDB7fs(h(CO&G{|1D9Z4LH<>~52WPr#=@a}%M~tgmlSqc^ zXQ5ZiR{E~FiM{>YL)xYnlXBIIMBAnVUD6E9v_3hYdss6pGabMjm7~P|^?a0Fy9k@i z9)j_Q)UhnrSFGRGJGMVfG83UeqCt6O>@?<16Ja6Aj1v3!)LOFvzm}~@+HacZG|4? zZ8$$TpY=JlplXi_x^zUs9Xk^`kemjMK8~oQtHkBL6kuYHG?Yk-gM`&vm>iOhdarUo z%X<;Gs&h5|#%VBhyCQV1-H$6%Y`A%5Dp7FkCsFPOpe&oilfEyDlX`t=D-{^ss{6(;bFthg-1Pt-g9u zOe=;w5uy1*W7<}AzPh*CoRjxmiP}CPD1T6evkBQx)A}vwkvS4{`6X$ZwAg@!$Zh76 z+*$awc@=hw=dzKr70|UVh_n0r0(b4!C7FKlIJ97lblsMRpY_s^;x(NM7`ulxSmXuI z<0%cK?L1M|3SO+;MEKnI8RqtMXFbQLO;xzp~q z581UV$>3Xdk{K3vfni1!3ct1DyOoMGVrLgQnDc{-OE95gmlx6gtUL@J9wi1#B~j*( z6)22P;(MP3}d|*J;SFS#5+9#vh08=bYG@fO9bO$aGAZ;~Jn%UK=H^&VfJLWTB(~HuU65!`eTy*=9Qp zqUIcp!`n>x!8!A|sxT+^(6$~dlz+jmG2e1F_bwScWR9%&Kje{$KUn8?z|Qb7x4HKR zs2OX3lIbY>`&}jUjyj@6s;Y1>o`+JY8d#fSOG~$EazO#%P_VO?HJgajjr&u{erpS1 bo + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/behaviors/slot/common/Dive_Right.xml b/behaviors/slot/common/Dive_Right.xml new file mode 100644 index 0000000..4078aea --- /dev/null +++ b/behaviors/slot/common/Dive_Right.xml @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/behaviors/slot/common/Flip.xml b/behaviors/slot/common/Flip.xml new file mode 100644 index 0000000..776fa80 --- /dev/null +++ b/behaviors/slot/common/Flip.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/behaviors/slot/common/Squat.xml b/behaviors/slot/common/Squat.xml new file mode 100644 index 0000000..d8893b0 --- /dev/null +++ b/behaviors/slot/common/Squat.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behaviors/slot/r0/Get_Up_Back.xml b/behaviors/slot/r0/Get_Up_Back.xml new file mode 100644 index 0000000..329871e --- /dev/null +++ b/behaviors/slot/r0/Get_Up_Back.xml @@ -0,0 +1,148 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/behaviors/slot/r0/Get_Up_Front.xml b/behaviors/slot/r0/Get_Up_Front.xml new file mode 100644 index 0000000..f3203c1 --- /dev/null +++ b/behaviors/slot/r0/Get_Up_Front.xml @@ -0,0 +1,220 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/behaviors/slot/r0/Kick_Motion.xml b/behaviors/slot/r0/Kick_Motion.xml new file mode 100644 index 0000000..931d8c0 --- /dev/null +++ b/behaviors/slot/r0/Kick_Motion.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behaviors/slot/r1/Get_Up_Back.xml b/behaviors/slot/r1/Get_Up_Back.xml new file mode 100644 index 0000000..0f0243f --- /dev/null +++ b/behaviors/slot/r1/Get_Up_Back.xml @@ -0,0 +1,148 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/behaviors/slot/r1/Get_Up_Front.xml b/behaviors/slot/r1/Get_Up_Front.xml new file mode 100644 index 0000000..f2d9ac1 --- /dev/null +++ b/behaviors/slot/r1/Get_Up_Front.xml @@ -0,0 +1,220 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/behaviors/slot/r1/Kick_Motion.xml b/behaviors/slot/r1/Kick_Motion.xml new file mode 100644 index 0000000..e671362 --- /dev/null +++ b/behaviors/slot/r1/Kick_Motion.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behaviors/slot/r2/Get_Up_Back.xml b/behaviors/slot/r2/Get_Up_Back.xml new file mode 100644 index 0000000..b3602ce --- /dev/null +++ b/behaviors/slot/r2/Get_Up_Back.xml @@ -0,0 +1,148 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/behaviors/slot/r2/Get_Up_Front.xml b/behaviors/slot/r2/Get_Up_Front.xml new file mode 100644 index 0000000..2c0c096 --- /dev/null +++ b/behaviors/slot/r2/Get_Up_Front.xml @@ -0,0 +1,220 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/behaviors/slot/r2/Kick_Motion.xml b/behaviors/slot/r2/Kick_Motion.xml new file mode 100644 index 0000000..8ce996c --- /dev/null +++ b/behaviors/slot/r2/Kick_Motion.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behaviors/slot/r3/Get_Up_Back.xml b/behaviors/slot/r3/Get_Up_Back.xml new file mode 100644 index 0000000..1ea2a3f --- /dev/null +++ b/behaviors/slot/r3/Get_Up_Back.xml @@ -0,0 +1,148 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/behaviors/slot/r3/Get_Up_Front.xml b/behaviors/slot/r3/Get_Up_Front.xml new file mode 100644 index 0000000..14b73a4 --- /dev/null +++ b/behaviors/slot/r3/Get_Up_Front.xml @@ -0,0 +1,220 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/behaviors/slot/r3/Kick_Motion.xml b/behaviors/slot/r3/Kick_Motion.xml new file mode 100644 index 0000000..17b0846 --- /dev/null +++ b/behaviors/slot/r3/Kick_Motion.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/behaviors/slot/r4/Get_Up_Back.xml b/behaviors/slot/r4/Get_Up_Back.xml new file mode 100644 index 0000000..0c36704 --- /dev/null +++ b/behaviors/slot/r4/Get_Up_Back.xml @@ -0,0 +1,148 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/behaviors/slot/r4/Get_Up_Front.xml b/behaviors/slot/r4/Get_Up_Front.xml new file mode 100644 index 0000000..2c0c096 --- /dev/null +++ b/behaviors/slot/r4/Get_Up_Front.xml @@ -0,0 +1,220 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/behaviors/slot/r4/Kick_Motion.xml b/behaviors/slot/r4/Kick_Motion.xml new file mode 100644 index 0000000..418628f --- /dev/null +++ b/behaviors/slot/r4/Kick_Motion.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/bundle/bundle.sh b/bundle/bundle.sh new file mode 100755 index 0000000..c57f1cd --- /dev/null +++ b/bundle/bundle.sh @@ -0,0 +1,83 @@ +#!/bin/bash + +# Call this script from any directory +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" + +# cd to main folder +cd "${SCRIPT_DIR}/.." + +rm -rf ./bundle/build +rm -rf ./bundle/dist + +onefile="--onefile" + +# bundle app, dependencies and data files into single executable + +pyinstaller \ +--add-data './world/commons/robots:world/commons/robots' \ +--add-data './behaviors/slot/common:behaviors/slot/common' \ +--add-data './behaviors/slot/r0:behaviors/slot/r0' \ +--add-data './behaviors/slot/r1:behaviors/slot/r1' \ +--add-data './behaviors/slot/r2:behaviors/slot/r2' \ +--add-data './behaviors/slot/r3:behaviors/slot/r3' \ +--add-data './behaviors/slot/r4:behaviors/slot/r4' \ +--add-data './behaviors/custom/Dribble/*.pkl:behaviors/custom/Dribble' \ +--add-data './behaviors/custom/Walk/*.pkl:behaviors/custom/Walk' \ +--add-data './behaviors/custom/Fall/*.pkl:behaviors/custom/Fall' \ +${onefile} --distpath ./bundle/dist/ --workpath ./bundle/build/ --noconfirm --name fcp Run_Player.py + +# start.sh + +cat > ./bundle/dist/start.sh << EOF +#!/bin/bash +export OMP_NUM_THREADS=1 + +host=\${1:-localhost} +port=\${2:-3100} + +for i in {1..11}; do + ./fcp -i \$host -p \$port -u \$i -t FCPortugal & +done +EOF + +# start_penalty.sh + +cat > ./bundle/dist/start_penalty.sh << EOF +#!/bin/bash +export OMP_NUM_THREADS=1 + +host=\${1:-localhost} +port=\${2:-3100} + +./fcp -i \$host -p \$port -u 1 -t FCPortugal -P 1 & +./fcp -i \$host -p \$port -u 11 -t FCPortugal -P 1 & +EOF + +# start_fat_proxy.sh + +cat > ./bundle/dist/start_fat_proxy.sh << EOF +#!/bin/bash +export OMP_NUM_THREADS=1 + +host=\${1:-localhost} +port=\${2:-3100} + +for i in {1..11}; do + ./fcp -i \$host -p \$port -u \$i -t FCPortugal -F 1 & +done +EOF + +# kill.sh + +cat > ./bundle/dist/kill.sh << EOF +#!/bin/bash +pkill -9 -e fcp +EOF + +# execution permission + +chmod a+x ./bundle/dist/start.sh +chmod a+x ./bundle/dist/start_penalty.sh +chmod a+x ./bundle/dist/start_fat_proxy.sh +chmod a+x ./bundle/dist/kill.sh + diff --git a/communication/Radio.py b/communication/Radio.py new file mode 100644 index 0000000..83adaf6 --- /dev/null +++ b/communication/Radio.py @@ -0,0 +1,306 @@ +from typing import List +from world.commons.Other_Robot import Other_Robot +from world.World import World +import numpy as np + +class Radio(): + ''' + map limits are hardcoded: + teammates/opponents positions (x,y) in ([-16,16],[-11,11]) + ball position (x,y) in ([-15,15],[-10,10]) + known server limitations: + claimed: all ascii from 0x20 to 0x7E except ' ', '(', ')' + bugs: + - ' or " clip the message + - '\' at the end or near another '\' + - ';' at beginning of message + ''' + # map limits are hardcoded: + + # lines, columns, half lines index, half cols index, (lines-1)/x_span, (cols-1)/y_span, combinations, combinations*2states, + TP = 321,221,160,110,10, 10,70941,141882 # teammate position + OP = 201,111,100,55, 6.25,5, 22311,44622 # opponent position + BP = 301,201,150,100,10, 10,60501 # ball position + SYMB = "!#$%&*+,-./0123456789:<=>?@ABCDEFGHIJKLMNOPQRSTUVWXYZ[]^_`abcdefghijklmnopqrstuvwxyz{|}~;" + SLEN = len(SYMB) + SYMB_TO_IDX = {ord(s):i for i,s in enumerate(SYMB)} + + + def __init__(self, world : World, commit_announcement) -> None: + self.world = world + self.commit_announcement = commit_announcement + r = world.robot + t = world.teammates + o = world.opponents + self.groups = ( # player team/unum, group has ball?, self in group? + [(t[9],t[10],o[6],o[7],o[8],o[9],o[10]), True ], # 2 teammates, 5 opponents, ball + [(t[0],t[1], t[2],t[3],t[4],t[5],t[6] ), False], # 7 teammates + [(t[7],t[8], o[0],o[1],o[2],o[3],o[4],o[5]), False] # 2 teammates, 6 opponents + ) + for g in self.groups: # add 'self in group?' + g.append(any(i.is_self for i in g[0])) + + def get_player_combination(self, pos, is_unknown, is_down, info): + ''' Returns combination (0-based) and number of possible combinations ''' + + if is_unknown: + return info[7]+1, info[7]+2 # return unknown combination + + x,y = pos[:2] + + if x < -17 or x > 17 or y < -12 or y > 12: + return info[7], info[7]+2 # return out of bounds combination (if it exceeds 1m in any axis) + + # convert to int to avoid overflow later + l = int(np.clip( round(info[4]*x+info[2]), 0, info[0]-1 )) # absorb out of bounds positions (up to 1m in each axis) + c = int(np.clip( round(info[5]*y+info[3]), 0, info[1]-1 )) + + return (l*info[1]+c)+(info[6] if is_down else 0), info[7]+2 # return valid combination + + + def get_ball_combination(self, x, y): + ''' Returns combination (0-based) and number of possible combinations ''' + + # if ball is out of bounds, we force it in + l = int(np.clip( round(Radio.BP[4]*x+Radio.BP[2]), 0, Radio.BP[0]-1 )) + c = int(np.clip( round(Radio.BP[5]*y+Radio.BP[3]), 0, Radio.BP[1]-1 )) + + return l*Radio.BP[1]+c, Radio.BP[6] # return valid combination + + def get_ball_position(self,comb): + + l = comb // Radio.BP[1] + c = comb % Radio.BP[1] + + return np.array([l/Radio.BP[4]-15, c/Radio.BP[5]-10, 0.042]) # assume ball is on ground + + def get_player_position(self,comb, info): + + if comb == info[7]: return -1 # player is out of bounds + if comb == info[7]+1: return -2 # player is in unknown location + + is_down = comb >= info[6] + if is_down: + comb -= info[6] + + l = comb // info[1] + c = comb % info[1] + + return l/info[4]-16, c/info[5]-11, is_down + + + def check_broadcast_requirements(self): + ''' + Check if broadcast group is valid + + Returns + ------- + ready : bool + True if all requirements are met + + Sequence: g0,g1,g2, ig0,ig1,ig2, iig0,iig1,iig2 (whole cycle: 0.36s) + igx means 'incomplete group', where <=1 element can be MIA recently + iigx means 'very incomplete group', where <=2 elements can be MIA recently + Rationale: prevent incomplete messages from monopolizing the broadcast space + + However: + - 1st round: when 0 group members are missing, that group will update 3 times every 0.36s + - 2nd round: when 1 group member is recently missing, that group will update 2 times every 0.36s + - 3rd round: when 2 group members are recently missing, that group will update 1 time every 0.36s + - when >2 group members are recently missing, that group will not be updated + + Players that have never been seen or heard are not considered for the 'recently missing'. + If there is only 1 group member since the beginning, the respective group can be updated, except in the 1st round. + In this way, the 1st round cannot be monopolized by clueless agents, which is important during games with 22 players. + ''' + + w = self.world + r = w.robot + ago40ms = w.time_local_ms - 40 + ago370ms = w.time_local_ms - 370 # maximum delay (up to 2 MIAs) is 360ms because radio has a delay of 20ms (otherwise max delay would be 340ms) + group : List[Other_Robot] + + idx9 = int((w.time_server * 25)+0.1) % 9 # sequence of 9 phases + max_MIA = idx9 // 3 # maximum number of MIA players (based on server time) + group_idx = idx9 % 3 # group number (based on server time) + group, has_ball, is_self_included = self.groups[group_idx] + + #============================================ 0. check if group is valid + + if has_ball and w.ball_abs_pos_last_update < ago40ms: # Ball is included and not up to date + return False + + if is_self_included and r.loc_last_update < ago40ms: # Oneself is included and unable to self-locate + return False + + # Get players that have been previously seen or heard but not recently + MIAs = [not ot.is_self and ot.state_last_update < ago370ms and ot.state_last_update > 0 for ot in group] + self.MIAs = [ot.state_last_update == 0 or MIAs[i] for i,ot in enumerate(group)] # add players that have never been seen + + if sum(MIAs) > max_MIA: # checking if number of recently missing members is not above threshold + return False + + # never seen before players are always ignored except when: + # - this is the 0 MIAs round (see explanation above) + # - all are MIA + if (max_MIA == 0 and any(self.MIAs)) or all(self.MIAs): + return False + + # Check for invalid members. Conditions: + # - Player is other and not MIA and: + # - last update was >40ms ago OR + # - last update did not include the head (head is important to provide state and accurate position) + + if any( + (not ot.is_self and not self.MIAs[i] and + (ot.state_last_update < ago40ms or ot.state_last_update==0 or len(ot.state_abs_pos)<3)# (last update: has no head or is old) + ) for i,ot in enumerate(group) + ): + return False + + return True + + + def broadcast(self): + ''' + Commit messages to teammates if certain conditions are met + Messages contain: positions/states of every moving entity + ''' + + if not self.check_broadcast_requirements(): + return + + w = self.world + ot : Other_Robot + + group_idx = int((w.time_server * 25)+0.1) % 3 # group number based on server time + group, has_ball, _ = self.groups[group_idx] + + #============================================ 1. create combination + + # add message number + combination = group_idx + no_of_combinations = 3 + + # add ball combination + if has_ball: + c, n = self.get_ball_combination(w.ball_abs_pos[0], w.ball_abs_pos[1]) + combination += c * no_of_combinations + no_of_combinations *= n + + + # add group combinations + for i,ot in enumerate(group): + c, n = self.get_player_combination(ot.state_abs_pos, # player position + self.MIAs[i], ot.state_fallen, # is unknown, is down + Radio.TP if ot.is_teammate else Radio.OP) # is teammate + combination += c * no_of_combinations + no_of_combinations *= n + + + assert(no_of_combinations < 9.61e38) # 88*89^19 (first character cannot be ';') + + #============================================ 2. create message + + # 1st msg symbol: ignore ';' due to server bug + msg = Radio.SYMB[combination % (Radio.SLEN-1)] + combination //= (Radio.SLEN-1) + + # following msg symbols + while combination: + msg += Radio.SYMB[combination % Radio.SLEN] + combination //= Radio.SLEN + + #============================================ 3. commit message + + self.commit_announcement(msg.encode()) # commit message + + + def receive(self, msg:bytearray): + w = self.world + r = w.robot + ago40ms = w.time_local_ms - 40 + ago110ms = w.time_local_ms - 110 + msg_time = w.time_local_ms - 20 # message was sent in the last step + + #============================================ 1. get combination + + # read first symbol, which cannot be ';' due to server bug + combination = Radio.SYMB_TO_IDX[msg[0]] + total_combinations = Radio.SLEN-1 + + if len(msg)>1: + for m in msg[1:]: + combination += total_combinations * Radio.SYMB_TO_IDX[m] + total_combinations *= Radio.SLEN + + #============================================ 2. get msg ID + + message_no = combination % 3 + combination //= 3 + group, has_ball, _ = self.groups[message_no] + + #============================================ 3. get data + + if has_ball: + ball_comb = combination % Radio.BP[6] + combination //= Radio.BP[6] + + players_combs = [] + ot : Other_Robot + for ot in group: + info = Radio.TP if ot.is_teammate else Radio.OP + players_combs.append( combination % (info[7]+2) ) + combination //= info[7]+2 + + #============================================ 4. update world + + if has_ball and w.ball_abs_pos_last_update < ago40ms: # update ball if it was not seen + time_diff = (msg_time - w.ball_abs_pos_last_update) / 1000 + ball = self.get_ball_position(ball_comb) + w.ball_abs_vel = (ball - w.ball_abs_pos) / time_diff + w.ball_abs_speed = np.linalg.norm(w.ball_abs_vel) + w.ball_abs_pos_last_update = msg_time # (error: 0-40 ms) + w.ball_abs_pos = ball + w.is_ball_abs_pos_from_vision = False + + for c, ot in zip(players_combs, group): + + # handle oneself case + if ot.is_self: + # the ball's position has a fair amount of noise, whether seen by us or other players + # but our self-locatization mechanism is usually much better than how others perceive us + if r.loc_last_update < ago110ms: # so we wait until we miss 2 visual steps + data = self.get_player_position(c, Radio.TP) + if type(data)==tuple: + x,y,is_down = data + r.loc_head_position[:2] = x,y # z is kept unchanged + r.loc_head_position_last_update = msg_time + r.radio_fallen_state = is_down + r.radio_last_update = msg_time + continue + + # do not update if other robot was recently seen + if ot.state_last_update >= ago40ms: + continue + + info = Radio.TP if ot.is_teammate else Radio.OP + data = self.get_player_position(c, info) + if type(data)==tuple: + x,y,is_down = data + p = np.array([x,y]) + + if ot.state_abs_pos is not None: # update the x & y components of the velocity + time_diff = (msg_time - ot.state_last_update) / 1000 + velocity = np.append( (p - ot.state_abs_pos[:2]) / time_diff, 0) # v.z = 0 + vel_diff = velocity - ot.state_filtered_velocity + if np.linalg.norm(vel_diff) < 4: # otherwise assume it was beamed + ot.state_filtered_velocity /= (ot.vel_decay,ot.vel_decay,1) # neutralize decay (except in the z-axis) + ot.state_filtered_velocity += ot.vel_filter * vel_diff + + ot.state_fallen = is_down + ot.state_last_update = msg_time + ot.state_body_parts_abs_pos = {"head":p} + ot.state_abs_pos = p + ot.state_horizontal_dist = np.linalg.norm(p - r.loc_head_position[:2]) + ot.state_ground_area = (p, 0.3 if is_down else 0.2) # not very precise, but we cannot see the robot \ No newline at end of file diff --git a/communication/Server_Comm.py b/communication/Server_Comm.py new file mode 100644 index 0000000..02d8c17 --- /dev/null +++ b/communication/Server_Comm.py @@ -0,0 +1,285 @@ +from communication.World_Parser import World_Parser +from itertools import count +from select import select +from sys import exit +from world.World import World +import socket +import time + +class Server_Comm(): + monitor_socket = None + + def __init__(self, host:str, agent_port:int, monitor_port:int, unum:int, robot_type:int, team_name:str, + world_parser:World_Parser, world:World, other_players, wait_for_server=True) -> None: + + self.BUFFER_SIZE = 8192 + self.rcv_buff = bytearray(self.BUFFER_SIZE) + self.send_buff = [] + self.world_parser = world_parser + self.unum = unum + + # During initialization, it's not clear whether we are on the left or right side + self._unofficial_beam_msg_left = "(agent (unum " + str(unum) + ") (team Left) (move " + self._unofficial_beam_msg_right = "(agent (unum " + str(unum) + ") (team Right) (move " + self.world = world + + self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM ) + + if wait_for_server: print("Waiting for server at ", host, ":", agent_port, sep="",end=".",flush=True) + while True: + try: + self.socket.connect((host, agent_port)) + print(end=" ") + break + except ConnectionRefusedError: + if not wait_for_server: + print("Server is down. Closing...") + exit() + time.sleep(1) + print(".",end="",flush=True) + print("Connected agent", unum, self.socket.getsockname()) + + self.send_immediate(b'(scene rsg/agent/nao/nao_hetero.rsg ' + str(robot_type).encode() + b')') + self._receive_async(other_players, True) + + self.send_immediate(b'(init (unum '+ str(unum).encode() + b') (teamname '+ team_name.encode() + b'))') + self._receive_async(other_players, False) + + # Repeat to guarantee that team side information is received + for _ in range(3): + # Eliminate advanced step by changing syn order (rcssserver3d protocol bug, usually seen for player 11) + self.send_immediate(b'(syn)') #if this syn is not needed, it will be discarded by the server + for p in other_players: + p.scom.send_immediate(b'(syn)') + for p in other_players: + p.scom.receive() + self.receive() + + if world.team_side_is_left == None: + print("\nError: server did not return a team side! Check server terminal!") + exit() + + # Monitor socket is shared by all agents on the same thread + if Server_Comm.monitor_socket is None and monitor_port is not None: + print("Connecting to server's monitor port at ", host, ":", monitor_port, sep="",end=".",flush=True) + Server_Comm.monitor_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM ) + Server_Comm.monitor_socket.connect((host, monitor_port)) + print("Done!") + + + + def _receive_async(self, other_players, first_pass) -> None: + '''Private function that receives asynchronous information during the initialization''' + + if not other_players: + self.receive() + return + + self.socket.setblocking(0) + if first_pass: print("Async agent",self.unum,"initialization", end="", flush=True) + + while True: + try: + print(".",end="",flush=True) + self.receive() + break + except: + pass + for p in other_players: + p.scom.send_immediate(b'(syn)') + for p in other_players: + p.scom.receive() + + self.socket.setblocking(1) + if not first_pass: print("Done!") + + + def receive(self, update=True): + + for i in count(): # parse all messages and perform value updates, but heavy computation is only done once at the end + try: + if self.socket.recv_into(self.rcv_buff, nbytes=4) != 4: raise ConnectionResetError() + msg_size = int.from_bytes(self.rcv_buff[:4], byteorder='big', signed=False) + if self.socket.recv_into(self.rcv_buff, nbytes=msg_size, flags=socket.MSG_WAITALL) != msg_size: raise ConnectionResetError() + except ConnectionResetError: + print("\nError: socket was closed by rcssserver3d!") + exit() + + self.world_parser.parse(self.rcv_buff[:msg_size]) + if len(select([self.socket],[],[], 0.0)[0]) == 0: break + + if update: + if i==1: self.world.log( "Server_Comm.py: The agent lost 1 packet! Is syncmode enabled?") + if i>1: self.world.log(f"Server_Comm.py: The agent lost {i} consecutive packets! Is syncmode disabled?") + self.world.update() + + if len(select([self.socket],[],[], 0.0)[0]) != 0: + self.world.log("Server_Comm.py: Received a new packet while on world.update()!") + self.receive() + + + def send_immediate(self, msg:bytes) -> None: + ''' Commit and send immediately ''' + try: + self.socket.send( (len(msg)).to_bytes(4,byteorder='big') + msg ) #Add message length in the first 4 bytes + except BrokenPipeError: + print("\nError: socket was closed by rcssserver3d!") + exit() + + + def send(self) -> None: + ''' Send all committed messages ''' + if len(select([self.socket],[],[], 0.0)[0]) == 0: + self.send_buff.append(b'(syn)') + self.send_immediate( b''.join(self.send_buff) ) + else: + self.world.log("Server_Comm.py: Received a new packet while thinking!") + self.send_buff = [] #clear buffer + + def commit(self, msg:bytes) -> None: + assert type(msg) == bytes, "Message must be of type Bytes!" + self.send_buff.append(msg) + + def commit_and_send(self, msg:bytes = b'') -> None: + self.commit(msg) + self.send() + + def clear_buffer(self) -> None: + self.send_buff = [] + + def commit_announcement(self, msg:bytes) -> None: + ''' + Say something to every player on the field. + Maximum 20 characters, ascii between 0x20, 0x7E except ' ', '(', ')' + Accepted: letters+numbers+symbols: !"#$%&'*+,-./:;<=>?@[\]^_`{|}~ + Message range: 50m (the field is 36m diagonally, so ignore this limitation) + A player can only hear a teammate's message every 2 steps (0.04s) + This ability exists independetly for messages from both teams + (i.e. our team cannot spam the other team to block their messages) + Messages from oneself are always heard + ''' + assert len(msg) <= 20 and type(msg) == bytes + self.commit(b'(say ' + msg + b')') + + def commit_pass_command(self) -> None: + ''' + Issue a pass command: + Conditions: + - The current playmode is PlayOn + - The agent is near the ball (default 0.5m) + - No opponents are near the ball (default 1m) + - The ball is stationary (default <0.05m/s) + - A certain amount of time has passed between pass commands + ''' + self.commit(b'(pass)') + + def commit_beam(self, pos2d, rot) -> None: + ''' + Official beam command that can be used in-game + This beam is affected by noise (unless it is disabled in the server configuration) + + Parameters + ---------- + pos2d : array_like + Absolute 2D position (negative X is always our half of the field, no matter our side) + rot : `int`/`float` + Player angle in degrees (0 points forward) + ''' + assert len(pos2d)==2, "The official beam command accepts only 2D positions!" + self.commit( f"(beam {pos2d[0]} {pos2d[1]} {rot})".encode() ) + + + def unofficial_beam(self, pos3d, rot) -> None: + ''' + Unofficial beam - it cannot be used in official matches + + Parameters + ---------- + pos3d : array_like + Absolute 3D position (negative X is always our half of the field, no matter our side) + rot : `int`/`float` + Player angle in degrees (0 points forward) + ''' + assert len(pos3d)==3, "The unofficial beam command accepts only 3D positions!" + + # there is no need to normalize the angle, the server accepts any angle + if self.world.team_side_is_left: + msg = f"{self._unofficial_beam_msg_left }{ pos3d[0]} { pos3d[1]} {pos3d[2]} {rot-90}))".encode() + else: + msg = f"{self._unofficial_beam_msg_right}{-pos3d[0]} {-pos3d[1]} {pos3d[2]} {rot+90}))".encode() + + self.monitor_socket.send( (len(msg)).to_bytes(4,byteorder='big') + msg ) + + def unofficial_kill_sim(self) -> None: + ''' Unofficial kill simulator command ''' + msg = b'(killsim)' + self.monitor_socket.send( (len(msg)).to_bytes(4,byteorder='big') + msg ) + + def unofficial_move_ball(self, pos3d, vel3d=(0,0,0)) -> None: + ''' + Unofficial command to move ball + info: ball radius = 0.042m + + Parameters + ---------- + pos3d : array_like + Absolute 3D position (negative X is always our half of the field, no matter our side) + vel3d : array_like + Absolute 3D velocity (negative X is always our half of the field, no matter our side) + ''' + assert len(pos3d)==3 and len(vel3d)==3, "To move the ball we need a 3D position and velocity" + + if self.world.team_side_is_left: + msg = f"(ball (pos { pos3d[0]} { pos3d[1]} {pos3d[2]}) (vel { vel3d[0]} { vel3d[1]} {vel3d[2]}))".encode() + else: + msg = f"(ball (pos {-pos3d[0]} {-pos3d[1]} {pos3d[2]}) (vel {-vel3d[0]} {-vel3d[1]} {vel3d[2]}))".encode() + + self.monitor_socket.send( (len(msg)).to_bytes(4,byteorder='big') + msg ) + + def unofficial_set_game_time(self, time_in_s : float) -> None: + ''' + Unofficial command to set the game time + e.g. unofficial_set_game_time(68.78) + + Parameters + ---------- + time_in_s : float + Game time in seconds + ''' + msg = f"(time {time_in_s})".encode() + self.monitor_socket.send( (len(msg)).to_bytes(4,byteorder='big') + msg ) + + def unofficial_set_play_mode(self, play_mode : str) -> None: + ''' + Unofficial command to set the play mode + e.g. unofficial_set_play_mode("PlayOn") + + Parameters + ---------- + play_mode : str + Play mode + ''' + msg = f"(playMode {play_mode})".encode() + self.monitor_socket.send( (len(msg)).to_bytes(4,byteorder='big') + msg ) + + def unofficial_kill_player(self, unum : int, team_side_is_left : bool) -> None: + ''' + Unofficial command to kill specific player + + Parameters + ---------- + unum : int + Uniform number + team_side_is_left : bool + True if player to kill belongs to left team + ''' + msg = f"(kill (unum {unum}) (team {'Left' if team_side_is_left else 'Right'}))".encode() + self.monitor_socket.send( (len(msg)).to_bytes(4,byteorder='big') + msg ) + + def close(self, close_monitor_socket = False): + ''' Close agent socket, and optionally the monitor socket (shared by players running on the same thread) ''' + self.socket.close() + if close_monitor_socket and Server_Comm.monitor_socket is not None: + Server_Comm.monitor_socket.close() + Server_Comm.monitor_socket = None + diff --git a/communication/World_Parser.py b/communication/World_Parser.py new file mode 100644 index 0000000..5359a3e --- /dev/null +++ b/communication/World_Parser.py @@ -0,0 +1,430 @@ +from math_ops.Math_Ops import Math_Ops as M +from world.Robot import Robot +from world.World import World +import math +import numpy as np + + +class World_Parser(): + def __init__(self, world:World, hear_callback) -> None: + self.LOG_PREFIX = "World_Parser.py: " + self.world = world + self.hear_callback = hear_callback + self.exp = None + self.depth = None + self.LEFT_SIDE_FLAGS = {b'F2L':(-15,-10,0), + b'F1L':(-15,+10,0), + b'F2R':(+15,-10,0), + b'F1R':(+15,+10,0), + b'G2L':(-15,-1.05,0.8), + b'G1L':(-15,+1.05,0.8), + b'G2R':(+15,-1.05,0.8), + b'G1R':(+15,+1.05,0.8)} #mapping between flag names and their corrected location, when playing on the left side + self.RIGHT_SIDE_FLAGS = {b'F2L':(+15,+10,0), + b'F1L':(+15,-10,0), + b'F2R':(-15,+10,0), + b'F1R':(-15,-10,0), + b'G2L':(+15,+1.05,0.8), + b'G1L':(+15,-1.05,0.8), + b'G2R':(-15,+1.05,0.8), + b'G1R':(-15,-1.05,0.8)} + self.play_mode_to_id = None + self.LEFT_PLAY_MODE_TO_ID = {"KickOff_Left":World.M_OUR_KICKOFF, "KickIn_Left":World.M_OUR_KICK_IN, "corner_kick_left":World.M_OUR_CORNER_KICK, + "goal_kick_left":World.M_OUR_GOAL_KICK, "free_kick_left":World.M_OUR_FREE_KICK, "pass_left":World.M_OUR_PASS, + "direct_free_kick_left": World.M_OUR_DIR_FREE_KICK, "Goal_Left": World.M_OUR_GOAL, "offside_left": World.M_OUR_OFFSIDE, + "KickOff_Right":World.M_THEIR_KICKOFF, "KickIn_Right":World.M_THEIR_KICK_IN, "corner_kick_right":World.M_THEIR_CORNER_KICK, + "goal_kick_right":World.M_THEIR_GOAL_KICK, "free_kick_right":World.M_THEIR_FREE_KICK, "pass_right":World.M_THEIR_PASS, + "direct_free_kick_right": World.M_THEIR_DIR_FREE_KICK, "Goal_Right": World.M_THEIR_GOAL, "offside_right": World.M_THEIR_OFFSIDE, + "BeforeKickOff": World.M_BEFORE_KICKOFF, "GameOver": World.M_GAME_OVER, "PlayOn": World.M_PLAY_ON } + self.RIGHT_PLAY_MODE_TO_ID = {"KickOff_Left":World.M_THEIR_KICKOFF, "KickIn_Left":World.M_THEIR_KICK_IN, "corner_kick_left":World.M_THEIR_CORNER_KICK, + "goal_kick_left":World.M_THEIR_GOAL_KICK, "free_kick_left":World.M_THEIR_FREE_KICK, "pass_left":World.M_THEIR_PASS, + "direct_free_kick_left": World.M_THEIR_DIR_FREE_KICK, "Goal_Left": World.M_THEIR_GOAL, "offside_left": World.M_THEIR_OFFSIDE, + "KickOff_Right":World.M_OUR_KICKOFF, "KickIn_Right":World.M_OUR_KICK_IN, "corner_kick_right":World.M_OUR_CORNER_KICK, + "goal_kick_right":World.M_OUR_GOAL_KICK, "free_kick_right":World.M_OUR_FREE_KICK, "pass_right":World.M_OUR_PASS, + "direct_free_kick_right": World.M_OUR_DIR_FREE_KICK, "Goal_Right": World.M_OUR_GOAL, "offside_right": World.M_OUR_OFFSIDE, + "BeforeKickOff": World.M_BEFORE_KICKOFF, "GameOver": World.M_GAME_OVER, "PlayOn": World.M_PLAY_ON } + + + def find_non_digit(self,start): + while True: + if (self.exp[start] < ord('0') or self.exp[start] > ord('9')) and self.exp[start] != ord('.'): return start + start+=1 + + def find_char(self,start,char): + while True: + if self.exp[start] == char : return start + start+=1 + + def read_float(self, start): + if self.exp[start:start+3] == b'nan': return float('nan'), start+3 #handle nan values (they exist) + end = self.find_non_digit(start+1) #we assume the first one is a digit or minus sign + try: + retval = float(self.exp[start:end]) + except: + self.world.log(f"{self.LOG_PREFIX}String to float conversion failed: {self.exp[start:end]} at msg[{start},{end}], \nMsg: {self.exp.decode()}") + retval = 0 + return retval, end + + def read_int(self, start): + end = self.find_non_digit(start+1) #we assume the first one is a digit or minus sign + return int(self.exp[start:end]), end + + def read_bytes(self, start): + end = start + while True: + if self.exp[end] == ord(' ') or self.exp[end] == ord(')'): break + end+=1 + + return self.exp[start:end], end + + def read_str(self, start): + b, end = self.read_bytes(start) + return b.decode(), end + + def get_next_tag(self, start): + min_depth = self.depth + while True: + if self.exp[start] == ord(")") : #monitor xml element depth + self.depth -= 1 + if min_depth > self.depth: min_depth = self.depth + elif self.exp[start] == ord("(") : break + start+=1 + if start >= len(self.exp): return None, start, 0 + + self.depth += 1 + start += 1 + end = self.find_char(start, ord(" ")) + return self.exp[start:end], end, min_depth + + + def parse(self, exp): + + self.exp = exp #used by other member functions + self.depth = 0 #xml element depth + self.world.step += 1 + self.world.line_count = 0 + self.world.robot.frp = dict() + self.world.flags_posts = dict() + self.world.flags_corners = dict() + self.world.vision_is_up_to_date = False + self.world.ball_is_visible = False + self.world.robot.feet_toes_are_touching = dict.fromkeys(self.world.robot.feet_toes_are_touching, False) + self.world.time_local_ms += World.STEPTIME_MS + + for p in self.world.teammates: p.is_visible = False + for p in self.world.opponents: p.is_visible = False + + tag, end, _ = self.get_next_tag(0) + + while end < len(exp): + + if tag==b'time': + while True: + tag, end, min_depth = self.get_next_tag(end) + if min_depth == 0: break + + if tag==b'now': + #last_time = self.world.time_server + self.world.time_server, end = self.read_float(end+1) + + #Test server time reliability + #increment = self.world.time_server - last_time + #if increment < 0.019: print ("down",last_time,self.world.time_server) + #if increment > 0.021: print ("up",last_time,self.world.time_server) + else: + self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'time': {tag} at {end}, \nMsg: {exp.decode()}") + + + elif tag==b'GS': + while True: + tag, end, min_depth = self.get_next_tag(end) + if min_depth == 0: break + + if tag==b'unum': + _, end = self.read_int(end+1) #We already know our unum + elif tag==b'team': + aux, end = self.read_str(end+1) + is_left = bool(aux == "left") + if self.world.team_side_is_left != is_left: + self.world.team_side_is_left = is_left + self.play_mode_to_id = self.LEFT_PLAY_MODE_TO_ID if is_left else self.RIGHT_PLAY_MODE_TO_ID + self.world.draw.set_team_side(not is_left) + self.world.team_draw.set_team_side(not is_left) + elif tag==b'sl': + if self.world.team_side_is_left: + self.world.goals_scored, end = self.read_int(end+1) + else: + self.world.goals_conceded, end = self.read_int(end+1) + elif tag==b'sr': + if self.world.team_side_is_left: + self.world.goals_conceded, end = self.read_int(end+1) + else: + self.world.goals_scored, end = self.read_int(end+1) + elif tag==b't': + self.world.time_game, end = self.read_float(end+1) + elif tag==b'pm': + aux, end = self.read_str(end+1) + if self.play_mode_to_id is not None: + self.world.play_mode = self.play_mode_to_id[aux] + else: + self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'GS': {tag} at {end}, \nMsg: {exp.decode()}") + + + elif tag==b'GYR': + while True: + tag, end, min_depth = self.get_next_tag(end) + if min_depth == 0: break + + ''' + The gyroscope measures the robot's torso angular velocity (rotation rate vector) + The angular velocity's orientation is given by the right-hand rule. + + Original reference frame: + X:left(-)/right(+) Y:back(-)/front(+) Z:down(-)/up(+) + + New reference frame: + X:back(-)/front(+) Y:right(-)/left(+) Z:down(-)/up(+) + + ''' + + if tag==b'n': + pass + elif tag==b'rt': + self.world.robot.gyro[1], end = self.read_float(end+1) + self.world.robot.gyro[0], end = self.read_float(end+1) + self.world.robot.gyro[2], end = self.read_float(end+1) + self.world.robot.gyro[1] *= -1 + else: + self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'GYR': {tag} at {end}, \nMsg: {exp.decode()}") + + + elif tag==b'ACC': + while True: + tag, end, min_depth = self.get_next_tag(end) + if min_depth == 0: break + + ''' + The accelerometer measures the acceleration relative to freefall. It will read zero during any type of free fall. + When at rest relative to the Earth's surface, it will indicate an upwards acceleration of 9.81m/s^2 (in SimSpark). + + Original reference frame: + X:left(-)/right(+) Y:back(-)/front(+) Z:down(-)/up(+) + + New reference frame: + X:back(-)/front(+) Y:right(-)/left(+) Z:down(-)/up(+) + ''' + + if tag==b'n': + pass + elif tag==b'a': + self.world.robot.acc[1], end = self.read_float(end+1) + self.world.robot.acc[0], end = self.read_float(end+1) + self.world.robot.acc[2], end = self.read_float(end+1) + self.world.robot.acc[1] *= -1 + else: + self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'ACC': {tag} at {end}, \nMsg: {exp.decode()}") + + + elif tag==b'HJ': + while True: + tag, end, min_depth = self.get_next_tag(end) + if min_depth == 0: break + + if tag==b'n': + joint_name, end = self.read_str(end+1) + joint_index = Robot.MAP_PERCEPTOR_TO_INDEX[joint_name] + elif tag==b'ax': + joint_angle, end = self.read_float(end+1) + + #Fix symmetry issues 2/4 (perceptors) + if joint_name in Robot.FIX_PERCEPTOR_SET: joint_angle = -joint_angle + + old_angle = self.world.robot.joints_position[joint_index] + self.world.robot.joints_speed[joint_index] = (joint_angle - old_angle) / World.STEPTIME * math.pi / 180 + self.world.robot.joints_position[joint_index] = joint_angle + else: + self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'HJ': {tag} at {end}, \nMsg: {exp.decode()}") + + elif tag==b'FRP': + while True: + tag, end, min_depth = self.get_next_tag(end) + if min_depth == 0: break + + ''' + The reference frame is used for the contact point and force vector applied to that point + Note: The force vector is applied to the foot, so it usually points up + + Original reference frame: + X:left(-)/right(+) Y:back(-)/front(+) Z:down(-)/up(+) + + New reference frame: + X:back(-)/front(+) Y:right(-)/left(+) Z:down(-)/up(+) + + ''' + + if tag==b'n': + foot_toe_id, end = self.read_str(end+1) + self.world.robot.frp[foot_toe_id] = foot_toe_ref = np.empty(6) + self.world.robot.feet_toes_last_touch[foot_toe_id] = self.world.time_local_ms + self.world.robot.feet_toes_are_touching[foot_toe_id] = True + elif tag==b'c': + foot_toe_ref[1], end = self.read_float(end+1) + foot_toe_ref[0], end = self.read_float(end+1) + foot_toe_ref[2], end = self.read_float(end+1) + foot_toe_ref[1] *= -1 + elif tag==b'f': + foot_toe_ref[4], end = self.read_float(end+1) + foot_toe_ref[3], end = self.read_float(end+1) + foot_toe_ref[5], end = self.read_float(end+1) + foot_toe_ref[4] *= -1 + else: + self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'FRP': {tag} at {end}, \nMsg: {exp.decode()}") + + + elif tag==b'See': + self.world.vision_is_up_to_date = True + self.world.vision_last_update = self.world.time_local_ms + + while True: + tag, end, min_depth = self.get_next_tag(end) + if min_depth == 0: break + + tag_bytes = bytes(tag) #since bytearray is not hashable, it cannot be used as key for dictionaries + + if tag==b'G1R' or tag==b'G2R' or tag==b'G1L' or tag==b'G2L': + _, end, _ = self.get_next_tag(end) + + c1, end = self.read_float(end+1) + c2, end = self.read_float(end+1) + c3, end = self.read_float(end+1) + + aux = self.LEFT_SIDE_FLAGS[tag_bytes] if self.world.team_side_is_left else self.RIGHT_SIDE_FLAGS[tag_bytes] + self.world.flags_posts[aux] = (c1,c2,c3) + + elif tag==b'F1R' or tag==b'F2R' or tag==b'F1L' or tag==b'F2L': + _, end, _ = self.get_next_tag(end) + + c1, end = self.read_float(end+1) + c2, end = self.read_float(end+1) + c3, end = self.read_float(end+1) + + aux = self.LEFT_SIDE_FLAGS[tag_bytes] if self.world.team_side_is_left else self.RIGHT_SIDE_FLAGS[tag_bytes] + self.world.flags_corners[aux] = (c1,c2,c3) + + elif tag==b'B': + _, end, _ = self.get_next_tag(end) + + self.world.ball_rel_head_sph_pos[0], end = self.read_float(end+1) + self.world.ball_rel_head_sph_pos[1], end = self.read_float(end+1) + self.world.ball_rel_head_sph_pos[2], end = self.read_float(end+1) + self.world.ball_rel_head_cart_pos = M.deg_sph2cart(self.world.ball_rel_head_sph_pos) + self.world.ball_is_visible = True + self.world.ball_last_seen = self.world.time_local_ms + + elif tag==b'mypos': + + self.world.robot.cheat_abs_pos[0], end = self.read_float(end+1) + self.world.robot.cheat_abs_pos[1], end = self.read_float(end+1) + self.world.robot.cheat_abs_pos[2], end = self.read_float(end+1) + + elif tag==b'myorien': + + self.world.robot.cheat_ori, end = self.read_float(end+1) + + elif tag==b'ballpos': + + c1, end = self.read_float(end+1) + c2, end = self.read_float(end+1) + c3, end = self.read_float(end+1) + + self.world.ball_cheat_abs_vel[0] = (c1 - self.world.ball_cheat_abs_pos[0]) / World.VISUALSTEP + self.world.ball_cheat_abs_vel[1] = (c2 - self.world.ball_cheat_abs_pos[1]) / World.VISUALSTEP + self.world.ball_cheat_abs_vel[2] = (c3 - self.world.ball_cheat_abs_pos[2]) / World.VISUALSTEP + + self.world.ball_cheat_abs_pos[0] = c1 + self.world.ball_cheat_abs_pos[1] = c2 + self.world.ball_cheat_abs_pos[2] = c3 + + elif tag==b'P': + + while True: + previous_depth = self.depth + previous_end = end + tag, end, min_depth = self.get_next_tag(end) + if min_depth < 2: #if =1 we are still inside 'See', if =0 we are already outside 'See' + end = previous_end #The "P" tag is special because it's the only variable particle inside 'See' + self.depth = previous_depth + break # we restore the previous tag, and let 'See' handle it + + if tag==b'team': + player_team, end = self.read_str(end+1) + is_teammate = bool(player_team == self.world.team_name) + if self.world.team_name_opponent is None and not is_teammate: #register opponent team name + self.world.team_name_opponent = player_team + elif tag==b'id': + player_id, end = self.read_int(end+1) + player = self.world.teammates[player_id-1] if is_teammate else self.world.opponents[player_id-1] + player.body_parts_cart_rel_pos = dict() #reset seen body parts + player.is_visible = True + elif tag==b'llowerarm' or tag==b'rlowerarm' or tag==b'lfoot' or tag==b'rfoot' or tag==b'head': + tag_str = tag.decode() + _, end, _ = self.get_next_tag(end) + + c1, end = self.read_float(end+1) + c2, end = self.read_float(end+1) + c3, end = self.read_float(end+1) + + if is_teammate: + self.world.teammates[player_id-1].body_parts_sph_rel_pos[tag_str] = (c1,c2,c3) + self.world.teammates[player_id-1].body_parts_cart_rel_pos[tag_str] = M.deg_sph2cart((c1,c2,c3)) + else: + self.world.opponents[player_id-1].body_parts_sph_rel_pos[tag_str] = (c1,c2,c3) + self.world.opponents[player_id-1].body_parts_cart_rel_pos[tag_str] = M.deg_sph2cart((c1,c2,c3)) + else: + self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'P': {tag} at {end}, \nMsg: {exp.decode()}") + + elif tag==b'L': + l = self.world.lines[self.world.line_count] + + _, end, _ = self.get_next_tag(end) + l[0], end = self.read_float(end+1) + l[1], end = self.read_float(end+1) + l[2], end = self.read_float(end+1) + _, end, _ = self.get_next_tag(end) + l[3], end = self.read_float(end+1) + l[4], end = self.read_float(end+1) + l[5], end = self.read_float(end+1) + + if np.isnan(l).any(): + self.world.log(f"{self.LOG_PREFIX}Received field line with NaNs {l}") + else: + self.world.line_count += 1 #accept field line if there are no NaNs + + else: + self.world.log(f"{self.LOG_PREFIX}Unknown tag inside 'see': {tag} at {end}, \nMsg: {exp.decode()}") + + + elif tag==b'hear': + + team_name, end = self.read_str(end+1) + + if team_name == self.world.team_name: # discard message if it's not from our team + + timestamp, end = self.read_float(end+1) + + if self.exp[end+1] == ord('s'): # this message was sent by oneself + direction, end = "self", end+5 + else: # this message was sent by teammate + direction, end = self.read_float(end+1) + + msg, end = self.read_bytes(end+1) + self.hear_callback(msg, direction, timestamp) + + + tag, end, _ = self.get_next_tag(end) + + + else: + self.world.log(f"{self.LOG_PREFIX}Unknown root tag: {tag} at {end}, \nMsg: {exp.decode()}") + tag, end, min_depth = self.get_next_tag(end) + diff --git a/cpp/a_star/Makefile b/cpp/a_star/Makefile new file mode 100644 index 0000000..d408f5f --- /dev/null +++ b/cpp/a_star/Makefile @@ -0,0 +1,14 @@ +src = $(wildcard *.cpp) +obj = $(src:.c=.o) + +CFLAGS = -O3 -shared -std=c++11 -fPIC -Wall $(PYBIND_INCLUDES) + +all: $(obj) + g++ $(CFLAGS) -o a_star.so $^ + +debug: $(filter-out lib_main.cpp,$(obj)) + g++ -O0 -std=c++14 -Wall -g -o debug.bin debug_main.cc $^ + +.PHONY: clean +clean: + rm -f $(obj) all diff --git a/cpp/a_star/a_star.cpp b/cpp/a_star/a_star.cpp new file mode 100644 index 0000000..969c6b1 --- /dev/null +++ b/cpp/a_star/a_star.cpp @@ -0,0 +1,918 @@ +#include "a_star.h" +#include "expansion_groups.h" +#include +#include +#include +#define SQRT2 1.414213562373095f +#define LINES 321 +#define COLS 221 +#define MAX_RADIUS 5 // obstacle max radius in meters +#define IN_GOAL_LINE 312 // target line when go_to_goal is 'true' (312 -> 15.2m) +using std::chrono::high_resolution_clock; +using std::chrono::duration_cast; +using std::chrono::microseconds; + +/* +Map dimensions: 32m*22m +col 0 ... col 220 + line 0 + --- our goal --- line 1 + | | line 2 + | | line 3 + |--------------| ... + | | line 317 + | | line 318 + -- their goal -- line 319 + line 320 + +[(H)ard wall: -3, (S)oft wall: -2, (E)mpty: 0, 0 < Cost < inf] +*/ + +// build board cost statically +#define H27 -3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3,-3 +#define S11 -2,-2,-2,-2,-2,-2,-2,-2,-2,-2,-2 +#define S19 S11,-2,-2,-2,-2,-2,-2,-2,-2 +#define S97 S11,S11,S11,S11,S11,S11,S11,S11,-2,-2,-2,-2,-2,-2,-2,-2,-2 +#define S98 S11,S11,S11,S11,S11,S11,S11,S11,-2,-2,-2,-2,-2,-2,-2,-2,-2,-2 +#define S221 S98,S98,S11,S11,-2,-2,-2 +#define E19 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 +#define E197 E19,E19,E19,E19,E19,E19,E19,E19,E19,E19,0,0,0,0,0,0,0 +#define L0 S221 // Line 0: soft W +#define L0_1 L0,L0 +#define L2 S97,H27,S97 // Line 2: soft W, (goal post, back net, goal post), soft W +#define L2_5 L2,L2,L2,L2 +#define L6 S98,-3,-3,-3,S19,-3,-3,-3,S98 // Line 6: soft W, goal post, soft W, goal post, soft W +#define L6_10 L6,L6,L6,L6,L6 +#define L11 S98,-2,-3,-3,S19,-3,-3,-2,S98 // Line 11:soft W, empty field, goal post, soft W, goal post,empty field, soft W +#define L12 S11,-2,E197,-2,S11 // Line 12:soft W, empty field, soft W + +#define L12x33 L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12,L12 +#define LIN12_308 L12x33,L12x33,L12x33,L12x33,L12x33,L12x33,L12x33,L12x33,L12x33 + +#define L309 S98,-2,-3,-3,E19,-3,-3,-2,S98 // Line 309: soft W, empty field, goal post, empty field, goal post,empty field, soft W +#define L310 S98,-3,-3,-3,E19,-3,-3,-3,S98 // Line 310: soft W, goal post, inside goal, goal post, soft W +#define L310_314 L310,L310,L310,L310,L310 + +using std::min; +using std::max; + + +#define MIN min_node +Node* min_node; // non-expanded node with lowest predicted total cost (f) + +namespace open{ + + Node* insert(Node* new_node, Node* root) { + + new_node->left = nullptr; + new_node->right = nullptr; + + // Empty BST, return without saving min_node + if(root == nullptr){ + new_node->up = nullptr; + MIN = new_node; // save min node for fast access + return new_node; + } + + // If new_node is the new min node + if(new_node->f < MIN->f){ + MIN->left = new_node; + new_node->up = MIN; + MIN = new_node; + return root; + } + + Node* node = root; + float key = new_node->f; + + while(true){ + if (key < node->f) + if(node->left == nullptr){ + node->left = new_node; + break; + }else{ + node = node->left; + } + else{ + if(node->right == nullptr){ + node->right = new_node; + break; + }else{ + node = node->right; + } + } + } + + new_node->up = node; + return root; + } + + + // Remove min node + Node* pop(Node* root) { + + // Minimum node can have right child, but not left child + if (MIN->right == nullptr){ + if(MIN == root){ //------(A)------ min node is root and has no children + return nullptr; // BST is empty + } + MIN->up->left = nullptr; //------(B)------ min node has no children but has parent + MIN = MIN->up; + }else{ + if(MIN == root){ //------(C)------ min node is root and has right child + MIN = MIN->right; + root = MIN; + root->up = nullptr; + + while(MIN->left != nullptr){ // update new min node + MIN = MIN->left; + } + + return root; // right child is now root + } + MIN->right->up = MIN->up; //------(D)------ min node has right child and parent + MIN->up->left = MIN->right; + + MIN = MIN->right; + while(MIN->left != nullptr){ // update new min node + MIN = MIN->left; + } + } + + return root; + } + + + // Remove specific node + Node* delete_node(Node* node, Node* root) { + + if(node == MIN){ // remove min node + return pop(root); + } + + if(node->left==nullptr and node->right==nullptr){ //------(A)------ node has no children (it can't be root, otherwise it would be min node) + // Redirect incoming connection + if(node->up->left == node){ node->up->left = nullptr; } + else{ node->up->right = nullptr; } + + }else if(node->left==nullptr){ //------(B)------ node has right child (it can't be root, otherwise it would be min node) + // Redirect incoming connections + node->right->up = node->up; + if(node->up->left == node){ node->up->left = node->right; } + else{ node->up->right = node->right; } + + }else if(node->right==nullptr){ //------(C)------ node has left child + if(node == root){ + node->left->up = nullptr; + return node->left; // left child becomes root + } + + // Redirect incoming connections (if not root) + node->left->up = node->up; + if(node->up->left == node){ node->up->left = node->left; } + else{ node->up->right = node->left; } + + }else{ //------(D)------ node has 2 children + Node *successor = node->right; + if(successor->left == nullptr){ //----- if successor is the node's right child (successor has no left child) + + //-------------- successor replaces node + + // Outgoing connections (successor's right child is not changed) + successor->left = node->left; + successor->up = node->up; // if node is root this is also ok + + // Incoming connections + node->left->up = successor; + + if(node == root){ return successor; } // successor becomes root + + // Incoming connections (if not root) + if(node->up->left == node){ node->up->left = successor; } + else{ node->up->right = successor; } + + }else{ //------ if successor is deeper (successor has no left child, and is itself a left child) + do{ + successor = successor->left; + }while(successor->left != nullptr); + + //-------------- Remove successor by redirecting its incoming connections + if(successor->right==nullptr){ // no children + successor->up->left = nullptr; + }else{ + successor->up->left = successor->right; + successor->right->up = successor->up; + } + + //-------------- successor replaces node + + // Outgoing connections + successor->left = node->left; + successor->right = node->right; + successor->up = node->up; // if node is root this is also ok + + // Incoming connections + node->left->up = successor; + node->right->up = successor; + + if(node == root){ return successor; } // successor becomes root + + // Incoming connections (if not root) + if(node->up->left == node){ node->up->left = successor; } + else{ node->up->right = successor; } + } + } + + return root; + } + + // Inorder Traversal + // void inorder(Node* root, Node* board) { + // if (root != nullptr) { + // // Traverse left + // inorder(root->left, board); + + // // Traverse root + // std::cout << (root-board)/COLS << " " << (root-board)%COLS << " -> "; + + // // Traverse right + // inorder(root->right, board); + + // } + // return; + // } +} + + + + +inline int x_to_line(float x){ + return int(fmaxf(0.f, fminf(10*x+160, 320.f)) + 0.5f); +} + +inline int y_to_col(float y){ + return int(fmaxf(0.f, fminf(10*y+110, 220.f)) + 0.5f); +} + +inline float diagonal_distance(bool go_to_goal, int line, int col, int end_l, int end_c){ + // diagonal distance - adapted from http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html + int dl, dc; + if( go_to_goal ){ + dl = abs(IN_GOAL_LINE - line); + if (col>119) { dc = col-119; } + else if (col<101) { dc = 101-col; } + else { dc = 0; } + }else{ + dl = abs(line - end_l); + dc = abs(col - end_c); + } + return (dl + dc) - 0.585786437626905f * min(dl,dc); +} + +inline Node* expand_child(Node* open_root, float cost, float wall_index, Node* curr_node, Node* board, int pos, int state, + bool go_to_goal, int line, int col, int end_l, int end_c, unsigned int* node_state, float extra ){ + // child can be as inaccessible as current pos (but there is a cost penalty to avoid inaccessible paths) + if(cost <= wall_index){ + cost = 100.f; + } + + // g (min cost from start to n) + float g = curr_node->g + extra + std::fmaxf(0.f,cost); // current cost + child distance + + Node* child = &board[pos]; + + // if child is already in the open set + if (state){ + if (g >= child->g){ + return open_root; // if not an improvement, we discard the new child + }else{ + open_root = open::delete_node(child, open_root); // if it is an improvement: remove reference, update it, add it again in correct order + } + }else{ + node_state[pos] = 1; + } + + // f (prediction of min total cost passing through n) + float f = g + diagonal_distance(go_to_goal,line,col,end_l,end_c); + + child->g = g; + child->f = f; + child->parent = curr_node; + return open::insert(child, open_root); +} + + +float final_path[2050]; +int final_path_size; + +inline void build_final_path(Node* const best_node, const Node* board, float status, const bool override_end=false, const float end_x=0, const float end_y=0){ + // Node* pt = best_node; + // while( pt != nullptr ){ + // int pos = pt - board; + // board_cost[pos] = -4; + // pt = pt->parent; + // } + // std::cout << "\n"; + // for(int l=l_min; l<=l_max; l++){ + // for(int c=c_min; c<=c_max; c++){ + // //[(H)ard wall: -3, (S)oft wall: -2, (E)mpty: 0, 0 < Cost < inf] + // //if (board[l][c].closed) std::cout << "o"; + // if (board_cost[l*COLS+c] == -3) std::cout << "h"; + // else if (board_cost[l*COLS+c] == -2) std::cout << "s"; + // else if (board_cost[l*COLS+c] == -4) std::cout << "."; + // else if (board_cost[l*COLS+c] == -1) std::cout << "g"; + // else if (board_cost[l*COLS+c] == 0 and node_state[l*COLS+c]==2) std::cout << "o"; + // else if (board_cost[l*COLS+c] == 0) std::cout << " "; + // //ele ainda nao sabe ler hard walls + // else std::cout << int(board_cost[l*COLS+c]+0.5f); + // } + // std::cout << "\n"; + // } + + // Using 'current_node' would suffice if A* reaches the objective (but 'best_node' works with impossible paths or timeout) + Node* ptr = best_node; + int counter=0; + do{ + ptr = ptr->parent; + counter++; + }while( ptr != nullptr ); + + final_path_size = min(counter*2,2048); + + ptr = best_node; + int i = final_path_size-1; + + // if enabled, replace end point with correct coordinates instead of discrete version + if(override_end){ + final_path[i--] = end_y; + final_path[i--] = end_x; + ptr = ptr->parent; + } + + for(; i>0;){ + final_path[i--] = ((ptr-board) % COLS)/10.f-11.f; // y + final_path[i--] = ((ptr-board) / COLS)/10.f-16.f; // x + ptr = ptr->parent; + } + + // add status (& increment path size) + final_path[final_path_size++] = status; // 0-success, 1-timeout, 2-impossible, 3-no obstacles(this one is not done in this function) + + // add cost (& increment path size) + final_path[final_path_size++] = best_node->g / 10.f; // min. A* cost from start to best_node + +} + + +/** + * @brief Returns true if line segment 'ab' intersects either goal (considering the unreachable area) + * - This function assumes that 'a' and 'b' are two points outside the unreachable goal area + * - Therefore, 'ab' must enter and exit the goal unreachable area + * - To detect this, we consider only the intersection of 'ab' and the goal outer borders (back+sides) + * - The front should already be covered by independent goal posts checks + */ +inline bool does_intersect_any_goal(float a_x, float a_y, float b_x, float b_y){ + + float ab_x = b_x - a_x; + float ab_y = b_y - a_y; + float k; + + if(ab_x != 0){ // Check if 'ab' and goal back is noncollinear (collinear intersections are ignored) + + k = (15.75-a_x) / ab_x; // a_x + ab_x*k = 15.75 + if (k >= 0 and k <= 1 and fabsf(a_y + ab_y * k) <= 1.25){ // collision (intersection_y = a_y + ab_y*k) + return true; + } + + k = (-15.75-a_x) / ab_x; // a_x + ab_x*k = -15.75 + if (k >= 0 and k <= 1 and fabsf(a_y + ab_y * k) <= 1.25){ // collision (intersection_y = a_y + ab_y*k) + return true; + } + } + + if(ab_y != 0){ // Check if 'ab' and goal sides are noncollinear (collinear intersections are ignored) + + k = (1.25-a_y) / ab_y; // a_y + ab_y*k = 1.25 + if( k >= 0 and k <= 1){ + float intersection_x_abs = fabsf(a_x + ab_x * k); + if( intersection_x_abs >= 15 and intersection_x_abs <= 15.75 ){ // check one side for both goals at same time + return true; + } + } + + k = (-1.25-a_y) / ab_y; // a_y + ab_y*k = -1.25 + if( k >= 0 and k <= 1){ + float intersection_x_abs = fabsf(a_x + ab_x * k); + if( intersection_x_abs >= 15 and intersection_x_abs <= 15.75 ){ // check one side for both goals at same time + return true; + } + } + } + + return false; +} + + +/** + * @brief Add space cushion near the midlines and endlines + */ +inline void add_space_cushion(float board_cost[]){ + + #define CUSHION_WIDTH 6 + + // opponent goal line + for(int i=0; i 0)){ + return true; + } + + + if (go_to_goal){ // This is a safe target. If it generates a collision with any goal post, we use A* instead. + end_x = 15.2; + end_y = max(-0.8f, min(start_y, 0.8f)); + }else{ // Restrict end coordinates to map + end_x = max(-16.f, min(end_x, 16.f)); + end_y = max(-11.f, min(end_y, 11.f)); + + // Let A* handle it if the end position is unreachable or (is nearly out of bounds && out of bounds is not allowed && not near end) + if(e_cost <= wall_index or (!is_near and e_cost > 0)){ + return true; + } + } + + + /** + * Check if path intersects either goal (considering the unreachable area) + * - at this point we know that 'start' and 'end' are reachable + * - Therefore, the path must enter and exit the goal unreachable area for an intersection to exist + * - To detect this, we consider only the intersection of the path and the goal outer borders (back+sides) + * - The front is covered next by goal posts checks + */ + if (does_intersect_any_goal(start_x, start_y, end_x, end_y)) { + return true; + } + + /** + * ----------------------- List all obstacles: given obstacles + goal posts + * note that including the goal posts in the given obstacles is not a bad idea since the default + * goal posts we add here provide no space cushion (which may be needed if the robot is not precise) + * values explanation: + * - goal post location (tested in simulator, collision with robot, robot is sideways to the goal post and arms are close to body) + * - hard radius (tested in the same way, robot collides when closer than 0.15m from goal post border) + * post radius 0.02 + agent radius 0.15 = 0.17 + * - largest radius (equivalent to hard radius, since there is no soft radius) + */ + + float obst[given_obst_size*4/5+16] = { + 15.02, 1.07, 0.17, 0.17, + 15.02, -1.07, 0.17, 0.17, + -15.02, 1.07, 0.17, 0.17, + -15.02, -1.07, 0.17, 0.17 + }; // x, y, hard radius, largest radius + + + int obst_size = 16; + + for(int i=0; istart center<->end + return true; + } + } + }else{ + + //-------------------- Normal case (start !~= end): the path is obstructed if it intersects any hard or soft circumference + + // for each obstacle: check if circle intersects line segment (start-end) + for(int ob=0; obcenter onto start->end + float sc_proj_y = se_y * sc_proj_scale; // projection of start->center onto start->end + + // check if projection falls on top of trajectory (start->projection = k * start->end) + float k = abs(se_x)>abs(se_y) ? sc_proj_x/se_x : sc_proj_y/se_y; // we use the largest dimension of start->end to avoid division by 0 + + if(k <= 0){ + if(sc_x*sc_x + sc_y*sc_y <= r_sq){ // check distance: center<->start + return true; + } + }else if(k >= 1){ + float ec_x = c_x - end_x; + float ec_y = c_y - end_y; + if(ec_x*ec_x + ec_y*ec_y <= r_sq){ // check distance: center<->end + return true; + } + }else{ + float proj_c_x = c_x - (sc_proj_x + start_x); + float proj_c_y = c_y - (sc_proj_y + start_y); + if(proj_c_x*proj_c_x + proj_c_y*proj_c_y <= r_sq){ // check distance: center<->projection + return true; + } + } + } + } + + float path_x = end_x - start_x; + float path_y = end_y - start_y; + + final_path_size = 6; + final_path[0] = start_x; + final_path[1] = start_y; + final_path[2] = end_x; + final_path[3] = end_y; + final_path[4] = 3; // status: 3-no obstacles + final_path[5] = sqrtf(path_x*path_x+path_y*path_y) + max(0.f, e_cost/10.f); // min. A* cost from start to end (e_cost is added even if start==end to help debug cell costs) + + return false; // no obstruction was found +} + +// opponent players + active player + restricted areas (from referee) +// data: +// [start x][start y] +// [allow out of bounds?][go to goal?] +// [optional target x][optional target y] +// [timeout] +// [x][y][hard radius][soft radius][force] +void astar(float params[], int params_size){ + + auto t1 = high_resolution_clock::now(); + + const float s_x = params[0]; // start x + const float s_y = params[1]; // start y + const bool allow_out_of_bounds = params[2]; + const int wall_index = allow_out_of_bounds ? -3 : -2; // (cost <= wall_index) means 'unreachable' + const bool go_to_goal = params[3]; + const float opt_t_x = params[4]; // optional target x + const float opt_t_y = params[5]; // optional target y + const int timeout_us = params[6]; + float* obstacles = ¶ms[7]; + int obst_size = params_size-7; // size of obstacles array + + //======================================================== Populate board 0: add field layout + float board_cost[LINES*COLS] = {L0_1,L2_5,L6_10,L11,LIN12_308,L309,L310_314,L2_5,L0_1}; + + if (!allow_out_of_bounds){ // add cost to getting near sideline or endline (except near goal) + add_space_cushion(board_cost); + } + + //======================================================== Check if path is obstructed + + if (!is_path_obstructed(s_x, s_y, opt_t_x, opt_t_y, obstacles, obst_size, go_to_goal, wall_index, board_cost)){ + return; // return if path is not obstructed + } + + //======================================================== Define board basics (start, end, limits) + + // if the start point is out of field, it is brought in + const int start_l = x_to_line(s_x); + const int start_c = y_to_col(s_y); + const int start_pos = start_l * COLS + start_c; + + // define objective (go to goal or a specific point) + int end_l, end_c; + if(!go_to_goal){ + end_l = x_to_line(opt_t_x); + end_c = y_to_col(opt_t_y); + }else{ + end_l = IN_GOAL_LINE; + } + + // define board limits considering the initial and final positions (and obstacles in the next section, and goals after that) + int l_min = min(start_l, end_l); + int l_max = max(start_l, end_l); + int c_min, c_max; + if(go_to_goal){ + c_min = min(start_c,119); + c_max = max(start_c,101); + }else{ + c_min = min(start_c, end_c); + c_max = max(start_c, end_c); + } + + if (!allow_out_of_bounds){ // workspace must contain a bit of empty field if out of bounds is not allowed + l_min = min(l_min, 306); + l_max = max(14, l_max); + c_min = min(c_min, 206); + c_max = max(14, c_max); + } + + //======================================================== Initialize A* + + Node* open_root = nullptr; + Node board[LINES*COLS]; + unsigned int node_state[LINES*COLS] = {0}; //0-unknown, 1-open, 2-closed + + //======================================================== Populate board 1: convert obstacles to cost + for(int ob=0; ob=0 and c>=0 and l=0 and c>=0 and l wall_index and cost < fr){ + board_cost[p] = fr; + } + } + } + + // adjust board limits if working area overlaps goal area (which includes walking margin) + if (c_max > 96 and c_min < 124){ // Otherwise it does not overlap any goal + if (l_max > 1 and l_min < 12 ){ // Overlaps our goal + l_max = max(12,l_max); // Extend working area to include our goal + l_min = min(l_min,1); + c_max = max(124,c_max); + c_min = min(c_min,96); + } + if (l_max > 308 and l_min < 319 ){ // Overlaps their goal + l_max = max(319,l_max); // Extend working area to include their goal + l_min = min(l_min,308); + c_max = max(124,c_max); + c_min = min(c_min,96); + } + } + + + //======================================================== Populate board 2: add objective + // Explanation: if objective is not accessible we do not add it to the map, although its reference still exists. + // Therefore, we know how far we are from that reference, but it cannot be reached. + // Even if we are on top of the objective, the idea is to get away, to the nearest valid position. + if(!go_to_goal){ + float *end_cost = &board_cost[end_l*COLS+end_c]; + if(*end_cost > wall_index){ + *end_cost = -1; + } + }else{ + for(int i=IN_GOAL_LINE*COLS+101; i<=IN_GOAL_LINE*COLS+119; i++){ + if(board_cost[i] > wall_index){ + board_cost[i] = -1; + } + } + } + + // add board limits as an additional restriction to workspace + l_min = max(0, l_min); + l_max = min(l_max, 320); + c_min = max(0, c_min); + c_max = min(c_max, 220); + + // add start node to open list (it will be closed right away, so there is not need to set it as open) + board[start_pos].g = 0; // This is needed to compute the cost of child nodes, but f is not needed because there are no comparisons with other nodes in the open BST + board[start_pos].parent = nullptr; //This is where the path ends + open_root = open::insert(&board[start_pos], open_root); + int measure_timeout=0; + Node* best_node = &board[start_pos]; // save best node based on distance to goal (useful if impossible/timeout to get best path) + + float best_node_dist = std::numeric_limits::max(); // infinite distance if start is itself unreachable + if(board_cost[start_pos] > wall_index){ + best_node_dist = diagonal_distance(go_to_goal,start_l,start_c,end_l,end_c); + } + + + + //======================================================== A* algorithm + while (open_root != nullptr){ + + // Get next best node (lowest predicted total cost (f)) + Node* curr_node = min_node; + const int curr_pos = curr_node - board; + const int curr_line = curr_pos / COLS; + const int curr_col = curr_pos % COLS; + const float curr_cost = board_cost[curr_pos]; + measure_timeout = (measure_timeout+1) & 31; // check timeout at every 32 iterations + + // save best node based on distance to goal (useful if impossible/timeout) + if(curr_cost > wall_index){ + float dd = diagonal_distance(go_to_goal,curr_line,curr_col,end_l,end_c); + if(best_node_dist > dd){ + best_node = curr_node; + best_node_dist = dd; + } + } + + + open_root = open::pop(open_root); + node_state[curr_pos] = 2; + + // Check if we reached objective + if( curr_cost == -1 ){ + // replace end point with correct coordinates instead of discrete version if the optional target was defined (not going to goal) + build_final_path(best_node, board, 0, !go_to_goal, opt_t_x, opt_t_y); + return; + } + if( measure_timeout==0 and duration_cast(high_resolution_clock::now() - t1).count() > timeout_us ){ + build_final_path(best_node, board, 1); + return; + } + + // Expand child nodes + bool rcol_ok = curr_col < c_max; + bool lcol_ok = curr_col > c_min; + + if(curr_line > l_min){ + int line = curr_line - 1; + int col = curr_col - 1; + int pos = curr_pos - COLS - 1; + float cost = board_cost[pos]; + int state = node_state[pos]; + + // check if not an obstacle and if node is not closed (child can be as inaccessible as current pos) + if (state!=2 and !(cost <= wall_index and cost < curr_cost) and lcol_ok){ + open_root = expand_child(open_root,cost,wall_index,curr_node,board,pos,state,go_to_goal,line,col,end_l,end_c,node_state,SQRT2); + } + + col++; + pos++; + cost = board_cost[pos]; + state = node_state[pos]; + + // check if not an obstacle and if node is not closed (child can be as inaccessible as current pos) + if (state!=2 and !(cost <= wall_index and cost < curr_cost)){ + open_root = expand_child(open_root,cost,wall_index,curr_node,board,pos,state,go_to_goal,line,col,end_l,end_c,node_state,1); + } + + col++; + pos++; + cost = board_cost[pos]; + state = node_state[pos]; + + // check if not an obstacle and if node is not closed (child can be as inaccessible as current pos) + if (state!=2 and !(cost <= wall_index and cost < curr_cost) and rcol_ok){ + open_root = expand_child(open_root,cost,wall_index,curr_node,board,pos,state,go_to_goal,line,col,end_l,end_c,node_state,SQRT2); + } + + } + + if(curr_line < l_max){ + int line = curr_line + 1; + int col = curr_col - 1; + int pos = curr_pos + COLS - 1; + float cost = board_cost[pos]; + int state = node_state[pos]; + + // check if not an obstacle and if node is not closed (child can be as inaccessible as current pos) + if (state!=2 and !(cost <= wall_index and cost < curr_cost) and lcol_ok){ + open_root = expand_child(open_root,cost,wall_index,curr_node,board,pos,state,go_to_goal,line,col,end_l,end_c,node_state,SQRT2); + } + + col++; + pos++; + cost = board_cost[pos]; + state = node_state[pos]; + + // check if not an obstacle and if node is not closed (child can be as inaccessible as current pos) + if (state!=2 and !(cost <= wall_index and cost < curr_cost)){ + open_root = expand_child(open_root,cost,wall_index,curr_node,board,pos,state,go_to_goal,line,col,end_l,end_c,node_state,1); + } + + col++; + pos++; + cost = board_cost[pos]; + state = node_state[pos]; + + // check if not an obstacle and if node is not closed (child can be as inaccessible as current pos) + if (state!=2 and !(cost <= wall_index and cost < curr_cost) and rcol_ok){ + open_root = expand_child(open_root,cost,wall_index,curr_node,board,pos,state,go_to_goal,line,col,end_l,end_c,node_state,SQRT2); + } + } + + + { + int col = curr_col - 1; + int pos = curr_pos - 1; + float cost = board_cost[pos]; + int state = node_state[pos]; + + // check if not an obstacle and if node is not closed (child can be as inaccessible as current pos) + if (state!=2 and !(cost <= wall_index and cost < curr_cost) and lcol_ok){ + open_root = expand_child(open_root,cost,wall_index,curr_node,board,pos,state,go_to_goal,curr_line,col,end_l,end_c,node_state,1); + } + + col+=2; + pos+=2; + cost = board_cost[pos]; + state = node_state[pos]; + + + // check if not an obstacle and if node is not closed (child can be as inaccessible as current pos) + if (state!=2 and !(cost <= wall_index and cost < curr_cost) and rcol_ok){ + open_root = expand_child(open_root,cost,wall_index,curr_node,board,pos,state,go_to_goal,curr_line,col,end_l,end_c,node_state,1); + } + } + + + } + + build_final_path(best_node, board, 2); + return; +} \ No newline at end of file diff --git a/cpp/a_star/a_star.h b/cpp/a_star/a_star.h new file mode 100644 index 0000000..16d0974 --- /dev/null +++ b/cpp/a_star/a_star.h @@ -0,0 +1,26 @@ +#pragma once + +/** + * FILENAME: a_star.h + * DESCRIPTION: custom A* pathfinding implementation, optimized for the soccer environment + * AUTHOR: Miguel Abreu (m.abreu@fe.up.pt) + * DATE: 2022 + */ + +struct Node{ + + //------------- BST parameters + Node* left; + Node* right; + Node* up; + + //------------- A* parameters + Node* parent; + float g; + float f; + +}; + +extern void astar(float params[], int params_size); +extern float final_path[2050]; +extern int final_path_size; \ No newline at end of file diff --git a/cpp/a_star/debug_main.cc b/cpp/a_star/debug_main.cc new file mode 100644 index 0000000..1cd5f83 --- /dev/null +++ b/cpp/a_star/debug_main.cc @@ -0,0 +1,37 @@ +#include "a_star.h" +#include +#include + +using std::chrono::high_resolution_clock; +using std::chrono::duration_cast; +using std::chrono::microseconds; + +std::chrono::_V2::system_clock::time_point t1,t2; + +float params[] = { + 15.78,-0.07, //start + 1,1, //out of bounds? go to goal? + 0,0, //target (if not go to goal) + 500000, // timeout + -10,0,1,5,5, + -10,1,1,7,10, + -10,-7,0,5,1 +}; +int params_size = sizeof(params)/sizeof(params[0]); + + +int main(){ + + t1 = high_resolution_clock::now(); + astar(params, params_size); + t2 = high_resolution_clock::now(); + + std::cout << duration_cast(t2 - t1).count() << "us (includes initialization)\n"; + + t1 = high_resolution_clock::now(); + astar(params, params_size); + t2 = high_resolution_clock::now(); + + std::cout << duration_cast(t2 - t1).count() << "us\n"; + +} \ No newline at end of file diff --git a/cpp/a_star/expansion_groups.h b/cpp/a_star/expansion_groups.h new file mode 100644 index 0000000..d8350b5 --- /dev/null +++ b/cpp/a_star/expansion_groups.h @@ -0,0 +1,6 @@ +const int expansion_positions_no = 7845; +const float expansion_pos_dist[7845] = {0.0,0.1,0.1,0.1,0.1,0.14142135623730953,0.14142135623730953,0.14142135623730953,0.14142135623730953,0.2,0.2,0.2,0.2,0.223606797749979,0.223606797749979,0.223606797749979,0.223606797749979,0.223606797749979,0.223606797749979,0.223606797749979,0.223606797749979,0.28284271247461906,0.28284271247461906,0.28284271247461906,0.28284271247461906,0.30000000000000004,0.30000000000000004,0.30000000000000004,0.30000000000000004,0.316227766016838,0.316227766016838,0.316227766016838,0.316227766016838,0.316227766016838,0.316227766016838,0.316227766016838,0.316227766016838,0.36055512754639896,0.36055512754639896,0.36055512754639896,0.36055512754639896,0.36055512754639896,0.36055512754639896,0.36055512754639896,0.36055512754639896,0.4,0.4,0.4,0.4,0.41231056256176607,0.41231056256176607,0.41231056256176607,0.41231056256176607,0.41231056256176607,0.41231056256176607,0.41231056256176607,0.41231056256176607,0.4242640687119285,0.4242640687119285,0.4242640687119285,0.4242640687119285,0.447213595499958,0.447213595499958,0.447213595499958,0.447213595499958,0.447213595499958,0.447213595499958,0.447213595499958,0.447213595499958,0.5,0.5,0.5,0.5,0.5,0.5,0.5,0.5,0.5,0.5,0.5,0.5,0.5099019513592785,0.5099019513592785,0.5099019513592785,0.5099019513592785,0.5099019513592785,0.5099019513592785,0.5099019513592785,0.5099019513592785,0.5385164807134504,0.5385164807134504,0.5385164807134504,0.5385164807134504,0.5385164807134504,0.5385164807134504,0.5385164807134504,0.5385164807134504,0.5656854249492381,0.5656854249492381,0.5656854249492381,0.5656854249492381,0.5830951894845301,0.5830951894845301,0.5830951894845301,0.5830951894845301,0.5830951894845301,0.5830951894845301,0.5830951894845301,0.5830951894845301,0.6000000000000001,0.6000000000000001,0.6000000000000001,0.6000000000000001,0.608276253029822,0.608276253029822,0.608276253029822,0.608276253029822,0.608276253029822,0.608276253029822,0.608276253029822,0.608276253029822,0.632455532033676,0.632455532033676,0.632455532033676,0.632455532033676,0.632455532033676,0.632455532033676,0.632455532033676,0.632455532033676,0.6403124237432849,0.6403124237432849,0.6403124237432849,0.6403124237432849,0.6403124237432849,0.6403124237432849,0.6403124237432849,0.6403124237432849,0.670820393249937,0.670820393249937,0.670820393249937,0.670820393249937,0.670820393249937,0.670820393249937,0.670820393249937,0.670820393249937,0.7000000000000001,0.7000000000000001,0.7000000000000001,0.7000000000000001,0.7071067811865476,0.7071067811865476,0.7071067811865476,0.7071067811865476,0.7071067811865476,0.7071067811865476,0.7071067811865476,0.7071067811865476,0.7071067811865476,0.7071067811865476,0.7071067811865476,0.7071067811865476,0.7211102550927979,0.7211102550927979,0.7211102550927979,0.7211102550927979,0.7211102550927979,0.7211102550927979,0.7211102550927979,0.7211102550927979,0.7280109889280518,0.7280109889280518,0.7280109889280518,0.7280109889280518,0.7280109889280518,0.7280109889280518,0.7280109889280518,0.7280109889280518,0.7615773105863909,0.7615773105863909,0.7615773105863909,0.7615773105863909,0.7615773105863909,0.7615773105863909,0.7615773105863909,0.7615773105863909,0.7810249675906654,0.7810249675906654,0.7810249675906654,0.7810249675906654,0.7810249675906654,0.7810249675906654,0.7810249675906654,0.7810249675906654,0.8,0.8,0.8,0.8,0.8062257748298549,0.8062257748298549,0.8062257748298549,0.8062257748298549,0.8062257748298549,0.8062257748298549,0.8062257748298549,0.8062257748298549,0.8062257748298549,0.8062257748298549,0.8062257748298549,0.8062257748298549,0.8062257748298549,0.8062257748298549,0.8062257748298549,0.8062257748298549,0.8246211251235321,0.8246211251235321,0.8246211251235321,0.8246211251235321,0.8246211251235321,0.8246211251235321,0.8246211251235321,0.8246211251235321,0.848528137423857,0.848528137423857,0.848528137423857,0.848528137423857,0.8544003745317531,0.8544003745317531,0.8544003745317531,0.8544003745317531,0.8544003745317531,0.8544003745317531,0.8544003745317531,0.8544003745317531,0.8602325267042628,0.8602325267042628,0.8602325267042628,0.8602325267042628,0.8602325267042628,0.8602325267042628,0.8602325267042628,0.8602325267042628,0.894427190999916,0.894427190999916,0.894427190999916,0.894427190999916,0.894427190999916,0.894427190999916,0.894427190999916,0.894427190999916,0.9,0.9,0.9,0.9,0.9055385138137417,0.9055385138137417,0.9055385138137417,0.9055385138137417,0.9055385138137417,0.9055385138137417,0.9055385138137417,0.9055385138137417,0.9219544457292888,0.9219544457292888,0.9219544457292888,0.9219544457292888,0.9219544457292888,0.9219544457292888,0.9219544457292888,0.9219544457292888,0.9219544457292888,0.9219544457292888,0.9219544457292888,0.9219544457292888,0.9219544457292888,0.9219544457292888,0.9219544457292888,0.9219544457292888,0.9433981132056604,0.9433981132056604,0.9433981132056604,0.9433981132056604,0.9433981132056604,0.9433981132056604,0.9433981132056604,0.9433981132056604,0.9486832980505139,0.9486832980505139,0.9486832980505139,0.9486832980505139,0.9486832980505139,0.9486832980505139,0.9486832980505139,0.9486832980505139,0.9848857801796105,0.9848857801796105,0.9848857801796105,0.9848857801796105,0.9848857801796105,0.9848857801796105,0.9848857801796105,0.9848857801796105,0.9899494936611666,0.9899494936611666,0.9899494936611666,0.9899494936611666,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.004987562112089,1.004987562112089,1.004987562112089,1.004987562112089,1.004987562112089,1.004987562112089,1.004987562112089,1.004987562112089,1.019803902718557,1.019803902718557,1.019803902718557,1.019803902718557,1.019803902718557,1.019803902718557,1.019803902718557,1.019803902718557,1.0295630140987002,1.0295630140987002,1.0295630140987002,1.0295630140987002,1.0295630140987002,1.0295630140987002,1.0295630140987002,1.0295630140987002,1.044030650891055,1.044030650891055,1.044030650891055,1.044030650891055,1.044030650891055,1.044030650891055,1.044030650891055,1.044030650891055,1.063014581273465,1.063014581273465,1.063014581273465,1.063014581273465,1.063014581273465,1.063014581273465,1.063014581273465,1.063014581273465,1.0770329614269007,1.0770329614269007,1.0770329614269007,1.0770329614269007,1.0770329614269007,1.0770329614269007,1.0770329614269007,1.0770329614269007,1.0816653826391969,1.0816653826391969,1.0816653826391969,1.0816653826391969,1.0816653826391969,1.0816653826391969,1.0816653826391969,1.0816653826391969,1.1,1.1,1.1,1.1,1.104536101718726,1.104536101718726,1.104536101718726,1.104536101718726,1.104536101718726,1.104536101718726,1.104536101718726,1.104536101718726,1.118033988749895,1.118033988749895,1.118033988749895,1.118033988749895,1.118033988749895,1.118033988749895,1.118033988749895,1.118033988749895,1.118033988749895,1.118033988749895,1.118033988749895,1.118033988749895,1.118033988749895,1.118033988749895,1.118033988749895,1.118033988749895,1.1313708498984762,1.1313708498984762,1.1313708498984762,1.1313708498984762,1.140175425099138,1.140175425099138,1.140175425099138,1.140175425099138,1.140175425099138,1.140175425099138,1.140175425099138,1.140175425099138,1.140175425099138,1.140175425099138,1.140175425099138,1.140175425099138,1.140175425099138,1.140175425099138,1.140175425099138,1.140175425099138,1.1661903789690602,1.1661903789690602,1.1661903789690602,1.1661903789690602,1.1661903789690602,1.1661903789690602,1.1661903789690602,1.1661903789690602,1.1704699910719627,1.1704699910719627,1.1704699910719627,1.1704699910719627,1.1704699910719627,1.1704699910719627,1.1704699910719627,1.1704699910719627,1.2000000000000002,1.2000000000000002,1.2000000000000002,1.2000000000000002,1.2041594578792296,1.2041594578792296,1.2041594578792296,1.2041594578792296,1.2041594578792296,1.2041594578792296,1.2041594578792296,1.2041594578792296,1.2041594578792296,1.2041594578792296,1.2041594578792296,1.2041594578792296,1.2041594578792296,1.2041594578792296,1.2041594578792296,1.2041594578792296,1.2083045973594573,1.2083045973594573,1.2083045973594573,1.2083045973594573,1.2083045973594573,1.2083045973594573,1.2083045973594573,1.2083045973594573,1.216552506059644,1.216552506059644,1.216552506059644,1.216552506059644,1.216552506059644,1.216552506059644,1.216552506059644,1.216552506059644,1.2206555615733703,1.2206555615733703,1.2206555615733703,1.2206555615733703,1.2206555615733703,1.2206555615733703,1.2206555615733703,1.2206555615733703,1.2369316876852983,1.2369316876852983,1.2369316876852983,1.2369316876852983,1.2369316876852983,1.2369316876852983,1.2369316876852983,1.2369316876852983,1.252996408614167,1.252996408614167,1.252996408614167,1.252996408614167,1.252996408614167,1.252996408614167,1.252996408614167,1.252996408614167,1.264911064067352,1.264911064067352,1.264911064067352,1.264911064067352,1.264911064067352,1.264911064067352,1.264911064067352,1.264911064067352,1.2727922061357857,1.2727922061357857,1.2727922061357857,1.2727922061357857,1.2806248474865698,1.2806248474865698,1.2806248474865698,1.2806248474865698,1.2806248474865698,1.2806248474865698,1.2806248474865698,1.2806248474865698,1.3,1.3,1.3,1.3,1.3,1.3,1.3,1.3,1.3,1.3,1.3,1.3,1.30384048104053,1.30384048104053,1.30384048104053,1.30384048104053,1.30384048104053,1.30384048104053,1.30384048104053,1.30384048104053,1.30384048104053,1.30384048104053,1.30384048104053,1.30384048104053,1.30384048104053,1.30384048104053,1.30384048104053,1.30384048104053,1.3152946437965907,1.3152946437965907,1.3152946437965907,1.3152946437965907,1.3152946437965907,1.3152946437965907,1.3152946437965907,1.3152946437965907,1.3341664064126335,1.3341664064126335,1.3341664064126335,1.3341664064126335,1.3341664064126335,1.3341664064126335,1.3341664064126335,1.3341664064126335,1.341640786499874,1.341640786499874,1.341640786499874,1.341640786499874,1.341640786499874,1.341640786499874,1.341640786499874,1.341640786499874,1.3453624047073711,1.3453624047073711,1.3453624047073711,1.3453624047073711,1.3453624047073711,1.3453624047073711,1.3453624047073711,1.3453624047073711,1.3601470508735445,1.3601470508735445,1.3601470508735445,1.3601470508735445,1.3601470508735445,1.3601470508735445,1.3601470508735445,1.3601470508735445,1.3601470508735445,1.3601470508735445,1.3601470508735445,1.3601470508735445,1.3601470508735445,1.3601470508735445,1.3601470508735445,1.3601470508735445,1.3892443989449805,1.3892443989449805,1.3892443989449805,1.3892443989449805,1.3892443989449805,1.3892443989449805,1.3892443989449805,1.3892443989449805,1.392838827718412,1.392838827718412,1.392838827718412,1.392838827718412,1.392838827718412,1.392838827718412,1.392838827718412,1.392838827718412,1.4000000000000001,1.4000000000000001,1.4000000000000001,1.4000000000000001,1.40356688476182,1.40356688476182,1.40356688476182,1.40356688476182,1.40356688476182,1.40356688476182,1.40356688476182,1.40356688476182,1.4142135623730951,1.4142135623730951,1.4142135623730951,1.4142135623730951,1.4142135623730951,1.4142135623730951,1.4142135623730951,1.4142135623730951,1.4142135623730951,1.4142135623730951,1.4142135623730951,1.4142135623730951,1.4212670403551897,1.4212670403551897,1.4212670403551897,1.4212670403551897,1.4212670403551897,1.4212670403551897,1.4212670403551897,1.4212670403551897,1.4317821063276355,1.4317821063276355,1.4317821063276355,1.4317821063276355,1.4317821063276355,1.4317821063276355,1.4317821063276355,1.4317821063276355,1.4317821063276355,1.4317821063276355,1.4317821063276355,1.4317821063276355,1.4317821063276355,1.4317821063276355,1.4317821063276355,1.4317821063276355,1.4422205101855958,1.4422205101855958,1.4422205101855958,1.4422205101855958,1.4422205101855958,1.4422205101855958,1.4422205101855958,1.4422205101855958,1.4560219778561037,1.4560219778561037,1.4560219778561037,1.4560219778561037,1.4560219778561037,1.4560219778561037,1.4560219778561037,1.4560219778561037,1.47648230602334,1.47648230602334,1.47648230602334,1.47648230602334,1.47648230602334,1.47648230602334,1.47648230602334,1.47648230602334,1.4866068747318506,1.4866068747318506,1.4866068747318506,1.4866068747318506,1.4866068747318506,1.4866068747318506,1.4866068747318506,1.4866068747318506,1.4866068747318506,1.4866068747318506,1.4866068747318506,1.4866068747318506,1.4866068747318506,1.4866068747318506,1.4866068747318506,1.4866068747318506,1.5,1.5,1.5,1.5,1.5,1.5,1.5,1.5,1.5,1.5,1.5,1.5,1.503329637837291,1.503329637837291,1.503329637837291,1.503329637837291,1.503329637837291,1.503329637837291,1.503329637837291,1.503329637837291,1.5132745950421558,1.5132745950421558,1.5132745950421558,1.5132745950421558,1.5132745950421558,1.5132745950421558,1.5132745950421558,1.5132745950421558,1.5231546211727818,1.5231546211727818,1.5231546211727818,1.5231546211727818,1.5231546211727818,1.5231546211727818,1.5231546211727818,1.5231546211727818,1.5264337522473748,1.5264337522473748,1.5264337522473748,1.5264337522473748,1.5264337522473748,1.5264337522473748,1.5264337522473748,1.5264337522473748,1.5297058540778357,1.5297058540778357,1.5297058540778357,1.5297058540778357,1.5297058540778357,1.5297058540778357,1.5297058540778357,1.5297058540778357,1.5524174696260025,1.5524174696260025,1.5524174696260025,1.5524174696260025,1.5524174696260025,1.5524174696260025,1.5524174696260025,1.5524174696260025,1.5556349186104046,1.5556349186104046,1.5556349186104046,1.5556349186104046,1.5620499351813308,1.5620499351813308,1.5620499351813308,1.5620499351813308,1.5620499351813308,1.5620499351813308,1.5620499351813308,1.5620499351813308,1.565247584249853,1.565247584249853,1.565247584249853,1.565247584249853,1.565247584249853,1.565247584249853,1.565247584249853,1.565247584249853,1.5811388300841898,1.5811388300841898,1.5811388300841898,1.5811388300841898,1.5811388300841898,1.5811388300841898,1.5811388300841898,1.5811388300841898,1.5811388300841898,1.5811388300841898,1.5811388300841898,1.5811388300841898,1.5811388300841898,1.5811388300841898,1.5811388300841898,1.5811388300841898,1.6,1.6,1.6,1.6,1.6031219541881399,1.6031219541881399,1.6031219541881399,1.6031219541881399,1.6031219541881399,1.6031219541881399,1.6031219541881399,1.6031219541881399,1.6124515496597098,1.6124515496597098,1.6124515496597098,1.6124515496597098,1.6124515496597098,1.6124515496597098,1.6124515496597098,1.6124515496597098,1.6124515496597098,1.6124515496597098,1.6124515496597098,1.6124515496597098,1.6124515496597098,1.6124515496597098,1.6124515496597098,1.6124515496597098,1.6155494421403511,1.6155494421403511,1.6155494421403511,1.6155494421403511,1.6155494421403511,1.6155494421403511,1.6155494421403511,1.6155494421403511,1.6278820596099708,1.6278820596099708,1.6278820596099708,1.6278820596099708,1.6278820596099708,1.6278820596099708,1.6278820596099708,1.6278820596099708,1.6278820596099708,1.6278820596099708,1.6278820596099708,1.6278820596099708,1.6278820596099708,1.6278820596099708,1.6278820596099708,1.6278820596099708,1.6401219466856727,1.6401219466856727,1.6401219466856727,1.6401219466856727,1.6401219466856727,1.6401219466856727,1.6401219466856727,1.6401219466856727,1.6492422502470643,1.6492422502470643,1.6492422502470643,1.6492422502470643,1.6492422502470643,1.6492422502470643,1.6492422502470643,1.6492422502470643,1.6552945357246849,1.6552945357246849,1.6552945357246849,1.6552945357246849,1.6552945357246849,1.6552945357246849,1.6552945357246849,1.6552945357246849,1.6643316977093239,1.6643316977093239,1.6643316977093239,1.6643316977093239,1.6643316977093239,1.6643316977093239,1.6643316977093239,1.6643316977093239,1.6763054614240211,1.6763054614240211,1.6763054614240211,1.6763054614240211,1.6763054614240211,1.6763054614240211,1.6763054614240211,1.6763054614240211,1.697056274847714,1.697056274847714,1.697056274847714,1.697056274847714,1.7000000000000002,1.7000000000000002,1.7000000000000002,1.7000000000000002,1.7000000000000002,1.7000000000000002,1.7000000000000002,1.7000000000000002,1.7000000000000002,1.7000000000000002,1.7000000000000002,1.7000000000000002,1.7029386365926404,1.7029386365926404,1.7029386365926404,1.7029386365926404,1.7029386365926404,1.7029386365926404,1.7029386365926404,1.7029386365926404,1.7029386365926404,1.7029386365926404,1.7029386365926404,1.7029386365926404,1.7029386365926404,1.7029386365926404,1.7029386365926404,1.7029386365926404,1.7088007490635062,1.7088007490635062,1.7088007490635062,1.7088007490635062,1.7088007490635062,1.7088007490635062,1.7088007490635062,1.7088007490635062,1.711724276862369,1.711724276862369,1.711724276862369,1.711724276862369,1.711724276862369,1.711724276862369,1.711724276862369,1.711724276862369,1.7204650534085255,1.7204650534085255,1.7204650534085255,1.7204650534085255,1.7204650534085255,1.7204650534085255,1.7204650534085255,1.7204650534085255,1.7262676501632068,1.7262676501632068,1.7262676501632068,1.7262676501632068,1.7262676501632068,1.7262676501632068,1.7262676501632068,1.7262676501632068,1.746424919657298,1.746424919657298,1.746424919657298,1.746424919657298,1.746424919657298,1.746424919657298,1.746424919657298,1.746424919657298,1.746424919657298,1.746424919657298,1.746424919657298,1.746424919657298,1.746424919657298,1.746424919657298,1.746424919657298,1.746424919657298,1.7492855684535902,1.7492855684535902,1.7492855684535902,1.7492855684535902,1.7492855684535902,1.7492855684535902,1.7492855684535902,1.7492855684535902,1.7691806012954132,1.7691806012954132,1.7691806012954132,1.7691806012954132,1.7691806012954132,1.7691806012954132,1.7691806012954132,1.7691806012954132,1.772004514666935,1.772004514666935,1.772004514666935,1.772004514666935,1.772004514666935,1.772004514666935,1.772004514666935,1.772004514666935,1.780449381476486,1.780449381476486,1.780449381476486,1.780449381476486,1.780449381476486,1.780449381476486,1.780449381476486,1.780449381476486,1.788854381999832,1.788854381999832,1.788854381999832,1.788854381999832,1.788854381999832,1.788854381999832,1.788854381999832,1.788854381999832,1.8,1.8,1.8,1.8,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8027756377319948,1.8110770276274835,1.8110770276274835,1.8110770276274835,1.8110770276274835,1.8110770276274835,1.8110770276274835,1.8110770276274835,1.8110770276274835,1.824828759089466,1.824828759089466,1.824828759089466,1.824828759089466,1.824828759089466,1.824828759089466,1.824828759089466,1.824828759089466,1.8357559750685821,1.8357559750685821,1.8357559750685821,1.8357559750685821,1.8357559750685821,1.8357559750685821,1.8357559750685821,1.8357559750685821,1.8384776310850235,1.8384776310850235,1.8384776310850235,1.8384776310850235,1.8384776310850235,1.8384776310850235,1.8384776310850235,1.8384776310850235,1.8384776310850235,1.8384776310850235,1.8384776310850235,1.8384776310850235,1.8439088914585775,1.8439088914585775,1.8439088914585775,1.8439088914585775,1.8439088914585775,1.8439088914585775,1.8439088914585775,1.8439088914585775,1.8439088914585775,1.8439088914585775,1.8439088914585775,1.8439088914585775,1.8439088914585775,1.8439088914585775,1.8439088914585775,1.8439088914585775,1.8601075237738276,1.8601075237738276,1.8601075237738276,1.8601075237738276,1.8601075237738276,1.8601075237738276,1.8601075237738276,1.8601075237738276,1.8681541692269406,1.8681541692269406,1.8681541692269406,1.8681541692269406,1.8681541692269406,1.8681541692269406,1.8681541692269406,1.8681541692269406,1.8788294228055937,1.8788294228055937,1.8788294228055937,1.8788294228055937,1.8788294228055937,1.8788294228055937,1.8788294228055937,1.8788294228055937,1.8867962264113207,1.8867962264113207,1.8867962264113207,1.8867962264113207,1.8867962264113207,1.8867962264113207,1.8867962264113207,1.8867962264113207,1.8973665961010278,1.8973665961010278,1.8973665961010278,1.8973665961010278,1.8973665961010278,1.8973665961010278,1.8973665961010278,1.8973665961010278,1.9000000000000001,1.9000000000000001,1.9000000000000001,1.9000000000000001,1.9026297590440446,1.9026297590440446,1.9026297590440446,1.9026297590440446,1.9026297590440446,1.9026297590440446,1.9026297590440446,1.9026297590440446,1.91049731745428,1.91049731745428,1.91049731745428,1.91049731745428,1.91049731745428,1.91049731745428,1.91049731745428,1.91049731745428,1.91049731745428,1.91049731745428,1.91049731745428,1.91049731745428,1.91049731745428,1.91049731745428,1.91049731745428,1.91049731745428,1.9209372712298547,1.9209372712298547,1.9209372712298547,1.9209372712298547,1.9209372712298547,1.9209372712298547,1.9209372712298547,1.9209372712298547,1.9235384061671343,1.9235384061671343,1.9235384061671343,1.9235384061671343,1.9235384061671343,1.9235384061671343,1.9235384061671343,1.9235384061671343,1.9235384061671343,1.9235384061671343,1.9235384061671343,1.9235384061671343,1.9235384061671343,1.9235384061671343,1.9235384061671343,1.9235384061671343,1.9313207915827968,1.9313207915827968,1.9313207915827968,1.9313207915827968,1.9313207915827968,1.9313207915827968,1.9313207915827968,1.9313207915827968,1.94164878389476,1.94164878389476,1.94164878389476,1.94164878389476,1.94164878389476,1.94164878389476,1.94164878389476,1.94164878389476,1.94164878389476,1.94164878389476,1.94164878389476,1.94164878389476,1.94164878389476,1.94164878389476,1.94164878389476,1.94164878389476,1.96468827043885,1.96468827043885,1.96468827043885,1.96468827043885,1.96468827043885,1.96468827043885,1.96468827043885,1.96468827043885,1.969771560359221,1.969771560359221,1.969771560359221,1.969771560359221,1.969771560359221,1.969771560359221,1.969771560359221,1.969771560359221,1.9723082923316022,1.9723082923316022,1.9723082923316022,1.9723082923316022,1.9723082923316022,1.9723082923316022,1.9723082923316022,1.9723082923316022,1.9798989873223332,1.9798989873223332,1.9798989873223332,1.9798989873223332,1.9849433241279208,1.9849433241279208,1.9849433241279208,1.9849433241279208,1.9849433241279208,1.9849433241279208,1.9849433241279208,1.9849433241279208,1.9924858845171276,1.9924858845171276,1.9924858845171276,1.9924858845171276,1.9924858845171276,1.9924858845171276,1.9924858845171276,1.9924858845171276,2.0,2.0,2.0,2.0,2.0,2.0,2.0,2.0,2.0,2.0,2.0,2.0,2.0024984394500787,2.0024984394500787,2.0024984394500787,2.0024984394500787,2.0024984394500787,2.0024984394500787,2.0024984394500787,2.0024984394500787,2.009975124224178,2.009975124224178,2.009975124224178,2.009975124224178,2.009975124224178,2.009975124224178,2.009975124224178,2.009975124224178,2.012461179749811,2.012461179749811,2.012461179749811,2.012461179749811,2.012461179749811,2.012461179749811,2.012461179749811,2.012461179749811,2.0223748416156684,2.0223748416156684,2.0223748416156684,2.0223748416156684,2.0223748416156684,2.0223748416156684,2.0223748416156684,2.0223748416156684,2.024845673131659,2.024845673131659,2.024845673131659,2.024845673131659,2.024845673131659,2.024845673131659,2.024845673131659,2.024845673131659,2.024845673131659,2.024845673131659,2.024845673131659,2.024845673131659,2.024845673131659,2.024845673131659,2.024845673131659,2.024845673131659,2.039607805437114,2.039607805437114,2.039607805437114,2.039607805437114,2.039607805437114,2.039607805437114,2.039607805437114,2.039607805437114,2.0518284528683193,2.0518284528683193,2.0518284528683193,2.0518284528683193,2.0518284528683193,2.0518284528683193,2.0518284528683193,2.0518284528683193,2.0591260281974004,2.0591260281974004,2.0591260281974004,2.0591260281974004,2.0591260281974004,2.0591260281974004,2.0591260281974004,2.0591260281974004,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0615528128088303,2.0808652046684815,2.0808652046684815,2.0808652046684815,2.0808652046684815,2.0808652046684815,2.0808652046684815,2.0808652046684815,2.0808652046684815,2.08806130178211,2.08806130178211,2.08806130178211,2.08806130178211,2.08806130178211,2.08806130178211,2.08806130178211,2.08806130178211,2.1,2.1,2.1,2.1,2.102379604162864,2.102379604162864,2.102379604162864,2.102379604162864,2.102379604162864,2.102379604162864,2.102379604162864,2.102379604162864,2.102379604162864,2.102379604162864,2.102379604162864,2.102379604162864,2.102379604162864,2.102379604162864,2.102379604162864,2.102379604162864,2.109502310972899,2.109502310972899,2.109502310972899,2.109502310972899,2.109502310972899,2.109502310972899,2.109502310972899,2.109502310972899,2.109502310972899,2.109502310972899,2.109502310972899,2.109502310972899,2.109502310972899,2.109502310972899,2.109502310972899,2.109502310972899,2.118962010041709,2.118962010041709,2.118962010041709,2.118962010041709,2.118962010041709,2.118962010041709,2.118962010041709,2.118962010041709,2.121320343559643,2.121320343559643,2.121320343559643,2.121320343559643,2.121320343559643,2.121320343559643,2.121320343559643,2.121320343559643,2.121320343559643,2.121320343559643,2.121320343559643,2.121320343559643,2.12602916254693,2.12602916254693,2.12602916254693,2.12602916254693,2.12602916254693,2.12602916254693,2.12602916254693,2.12602916254693,2.137755832643195,2.137755832643195,2.137755832643195,2.137755832643195,2.137755832643195,2.137755832643195,2.137755832643195,2.137755832643195,2.1400934559032696,2.1400934559032696,2.1400934559032696,2.1400934559032696,2.1400934559032696,2.1400934559032696,2.1400934559032696,2.1400934559032696,2.147091055358389,2.147091055358389,2.147091055358389,2.147091055358389,2.147091055358389,2.147091055358389,2.147091055358389,2.147091055358389,2.1540659228538015,2.1540659228538015,2.1540659228538015,2.1540659228538015,2.1540659228538015,2.1540659228538015,2.1540659228538015,2.1540659228538015,2.1587033144922905,2.1587033144922905,2.1587033144922905,2.1587033144922905,2.1587033144922905,2.1587033144922905,2.1587033144922905,2.1587033144922905,2.1633307652783937,2.1633307652783937,2.1633307652783937,2.1633307652783937,2.1633307652783937,2.1633307652783937,2.1633307652783937,2.1633307652783937,2.1840329667841556,2.1840329667841556,2.1840329667841556,2.1840329667841556,2.1840329667841556,2.1840329667841556,2.1840329667841556,2.1840329667841556,2.193171219946131,2.193171219946131,2.193171219946131,2.193171219946131,2.193171219946131,2.193171219946131,2.193171219946131,2.193171219946131,2.193171219946131,2.193171219946131,2.193171219946131,2.193171219946131,2.193171219946131,2.193171219946131,2.193171219946131,2.193171219946131,2.195449840010015,2.195449840010015,2.195449840010015,2.195449840010015,2.195449840010015,2.195449840010015,2.195449840010015,2.195449840010015,2.2,2.2,2.2,2.2,2.202271554554524,2.202271554554524,2.202271554554524,2.202271554554524,2.202271554554524,2.202271554554524,2.202271554554524,2.202271554554524,2.202271554554524,2.202271554554524,2.202271554554524,2.202271554554524,2.202271554554524,2.202271554554524,2.202271554554524,2.202271554554524,2.209072203437452,2.209072203437452,2.209072203437452,2.209072203437452,2.209072203437452,2.209072203437452,2.209072203437452,2.209072203437452,2.2135943621178655,2.2135943621178655,2.2135943621178655,2.2135943621178655,2.2135943621178655,2.2135943621178655,2.2135943621178655,2.2135943621178655,2.220360331117452,2.220360331117452,2.220360331117452,2.220360331117452,2.220360331117452,2.220360331117452,2.220360331117452,2.220360331117452,2.220360331117452,2.220360331117452,2.220360331117452,2.220360331117452,2.220360331117452,2.220360331117452,2.220360331117452,2.220360331117452,2.23606797749979,2.23606797749979,2.23606797749979,2.23606797749979,2.23606797749979,2.23606797749979,2.23606797749979,2.23606797749979,2.23606797749979,2.23606797749979,2.23606797749979,2.23606797749979,2.23606797749979,2.23606797749979,2.23606797749979,2.23606797749979,2.247220505424423,2.247220505424423,2.247220505424423,2.247220505424423,2.247220505424423,2.247220505424423,2.247220505424423,2.247220505424423,2.247220505424423,2.247220505424423,2.247220505424423,2.247220505424423,2.247220505424423,2.247220505424423,2.247220505424423,2.247220505424423,2.2561028345356955,2.2561028345356955,2.2561028345356955,2.2561028345356955,2.2561028345356955,2.2561028345356955,2.2561028345356955,2.2561028345356955,2.2627416997969525,2.2627416997969525,2.2627416997969525,2.2627416997969525,2.267156809750927,2.267156809750927,2.267156809750927,2.267156809750927,2.267156809750927,2.267156809750927,2.267156809750927,2.267156809750927,2.280350850198276,2.280350850198276,2.280350850198276,2.280350850198276,2.280350850198276,2.280350850198276,2.280350850198276,2.280350850198276,2.280350850198276,2.280350850198276,2.280350850198276,2.280350850198276,2.280350850198276,2.280350850198276,2.280350850198276,2.280350850198276,2.2825424421026654,2.2825424421026654,2.2825424421026654,2.2825424421026654,2.2825424421026654,2.2825424421026654,2.2825424421026654,2.2825424421026654,2.2847319317591728,2.2847319317591728,2.2847319317591728,2.2847319317591728,2.2847319317591728,2.2847319317591728,2.2847319317591728,2.2847319317591728,2.3000000000000003,2.3000000000000003,2.3000000000000003,2.3000000000000003,2.3021728866442674,2.3021728866442674,2.3021728866442674,2.3021728866442674,2.3021728866442674,2.3021728866442674,2.3021728866442674,2.3021728866442674,2.3021728866442674,2.3021728866442674,2.3021728866442674,2.3021728866442674,2.3021728866442674,2.3021728866442674,2.3021728866442674,2.3021728866442674,2.3086792761230392,2.3086792761230392,2.3086792761230392,2.3086792761230392,2.3086792761230392,2.3086792761230392,2.3086792761230392,2.3086792761230392,2.3086792761230392,2.3086792761230392,2.3086792761230392,2.3086792761230392,2.3086792761230392,2.3086792761230392,2.3086792761230392,2.3086792761230392,2.3194827009486403,2.3194827009486403,2.3194827009486403,2.3194827009486403,2.3194827009486403,2.3194827009486403,2.3194827009486403,2.3194827009486403,2.3259406699226015,2.3259406699226015,2.3259406699226015,2.3259406699226015,2.3259406699226015,2.3259406699226015,2.3259406699226015,2.3259406699226015,2.3323807579381204,2.3323807579381204,2.3323807579381204,2.3323807579381204,2.3323807579381204,2.3323807579381204,2.3323807579381204,2.3323807579381204,2.3345235059857505,2.3345235059857505,2.3345235059857505,2.3345235059857505,2.3345235059857505,2.3345235059857505,2.3345235059857505,2.3345235059857505,2.3345235059857505,2.3345235059857505,2.3345235059857505,2.3345235059857505,2.3345235059857505,2.3345235059857505,2.3345235059857505,2.3345235059857505,2.3409399821439254,2.3409399821439254,2.3409399821439254,2.3409399821439254,2.3409399821439254,2.3409399821439254,2.3409399821439254,2.3409399821439254,2.3430749027719964,2.3430749027719964,2.3430749027719964,2.3430749027719964,2.3430749027719964,2.3430749027719964,2.3430749027719964,2.3430749027719964,2.353720459187964,2.353720459187964,2.353720459187964,2.353720459187964,2.353720459187964,2.353720459187964,2.353720459187964,2.353720459187964,2.3600847442411896,2.3600847442411896,2.3600847442411896,2.3600847442411896,2.3600847442411896,2.3600847442411896,2.3600847442411896,2.3600847442411896,2.3706539182259396,2.3706539182259396,2.3706539182259396,2.3706539182259396,2.3706539182259396,2.3706539182259396,2.3706539182259396,2.3706539182259396,2.3769728648009427,2.3769728648009427,2.3769728648009427,2.3769728648009427,2.3769728648009427,2.3769728648009427,2.3769728648009427,2.3769728648009427,2.3769728648009427,2.3769728648009427,2.3769728648009427,2.3769728648009427,2.3769728648009427,2.3769728648009427,2.3769728648009427,2.3769728648009427,2.3853720883753127,2.3853720883753127,2.3853720883753127,2.3853720883753127,2.3853720883753127,2.3853720883753127,2.3853720883753127,2.3853720883753127,2.4000000000000004,2.4000000000000004,2.4000000000000004,2.4000000000000004,2.4020824298928627,2.4020824298928627,2.4020824298928627,2.4020824298928627,2.4020824298928627,2.4020824298928627,2.4020824298928627,2.4020824298928627,2.4041630560342617,2.4041630560342617,2.4041630560342617,2.4041630560342617,2.4041630560342617,2.4041630560342617,2.4041630560342617,2.4041630560342617,2.4041630560342617,2.4041630560342617,2.4041630560342617,2.4041630560342617,2.4083189157584592,2.4083189157584592,2.4083189157584592,2.4083189157584592,2.4083189157584592,2.4083189157584592,2.4083189157584592,2.4083189157584592,2.4083189157584592,2.4083189157584592,2.4083189157584592,2.4083189157584592,2.4083189157584592,2.4083189157584592,2.4083189157584592,2.4083189157584592,2.4166091947189146,2.4166091947189146,2.4166091947189146,2.4166091947189146,2.4166091947189146,2.4166091947189146,2.4166091947189146,2.4166091947189146,2.4186773244895647,2.4186773244895647,2.4186773244895647,2.4186773244895647,2.4186773244895647,2.4186773244895647,2.4186773244895647,2.4186773244895647,2.4186773244895647,2.4186773244895647,2.4186773244895647,2.4186773244895647,2.4186773244895647,2.4186773244895647,2.4186773244895647,2.4186773244895647,2.420743687382041,2.420743687382041,2.420743687382041,2.420743687382041,2.420743687382041,2.420743687382041,2.420743687382041,2.420743687382041,2.433105012119288,2.433105012119288,2.433105012119288,2.433105012119288,2.433105012119288,2.433105012119288,2.433105012119288,2.433105012119288,2.4351591323771844,2.4351591323771844,2.4351591323771844,2.4351591323771844,2.4351591323771844,2.4351591323771844,2.4351591323771844,2.4351591323771844,2.4413111231467406,2.4413111231467406,2.4413111231467406,2.4413111231467406,2.4413111231467406,2.4413111231467406,2.4413111231467406,2.4413111231467406,2.451530134426253,2.451530134426253,2.451530134426253,2.451530134426253,2.451530134426253,2.451530134426253,2.451530134426253,2.451530134426253,2.459674775249769,2.459674775249769,2.459674775249769,2.459674775249769,2.459674775249769,2.459674775249769,2.459674775249769,2.459674775249769,2.4698178070456938,2.4698178070456938,2.4698178070456938,2.4698178070456938,2.4698178070456938,2.4698178070456938,2.4698178070456938,2.4698178070456938,2.4698178070456938,2.4698178070456938,2.4698178070456938,2.4698178070456938,2.4698178070456938,2.4698178070456938,2.4698178070456938,2.4698178070456938,2.4738633753705965,2.4738633753705965,2.4738633753705965,2.4738633753705965,2.4738633753705965,2.4738633753705965,2.4738633753705965,2.4738633753705965,2.4758836806279896,2.4758836806279896,2.4758836806279896,2.4758836806279896,2.4758836806279896,2.4758836806279896,2.4758836806279896,2.4758836806279896,2.4839484696748446,2.4839484696748446,2.4839484696748446,2.4839484696748446,2.4839484696748446,2.4839484696748446,2.4839484696748446,2.4839484696748446,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5,2.5019992006393608,2.5019992006393608,2.5019992006393608,2.5019992006393608,2.5019992006393608,2.5019992006393608,2.5019992006393608,2.5019992006393608,2.505992817228334,2.505992817228334,2.505992817228334,2.505992817228334,2.505992817228334,2.505992817228334,2.505992817228334,2.505992817228334,2.507987240796891,2.507987240796891,2.507987240796891,2.507987240796891,2.507987240796891,2.507987240796891,2.507987240796891,2.507987240796891,2.507987240796891,2.507987240796891,2.507987240796891,2.507987240796891,2.507987240796891,2.507987240796891,2.507987240796891,2.507987240796891,2.5179356624028344,2.5179356624028344,2.5179356624028344,2.5179356624028344,2.5179356624028344,2.5179356624028344,2.5179356624028344,2.5179356624028344,2.5238858928247927,2.5238858928247927,2.5238858928247927,2.5238858928247927,2.5238858928247927,2.5238858928247927,2.5238858928247927,2.5238858928247927,2.529822128134704,2.529822128134704,2.529822128134704,2.529822128134704,2.529822128134704,2.529822128134704,2.529822128134704,2.529822128134704,2.5317977802344327,2.5317977802344327,2.5317977802344327,2.5317977802344327,2.5317977802344327,2.5317977802344327,2.5317977802344327,2.5317977802344327,2.5455844122715714,2.5455844122715714,2.5455844122715714,2.5455844122715714,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5495097567963927,2.5553864678361276,2.5553864678361276,2.5553864678361276,2.5553864678361276,2.5553864678361276,2.5553864678361276,2.5553864678361276,2.5553864678361276,2.5612496949731396,2.5612496949731396,2.5612496949731396,2.5612496949731396,2.5612496949731396,2.5612496949731396,2.5612496949731396,2.5612496949731396,2.5632011235952596,2.5632011235952596,2.5632011235952596,2.5632011235952596,2.5632011235952596,2.5632011235952596,2.5632011235952596,2.5632011235952596,2.5709920264364885,2.5709920264364885,2.5709920264364885,2.5709920264364885,2.5709920264364885,2.5709920264364885,2.5709920264364885,2.5709920264364885,2.5806975801127883,2.5806975801127883,2.5806975801127883,2.5806975801127883,2.5806975801127883,2.5806975801127883,2.5806975801127883,2.5806975801127883,2.5942243542145693,2.5942243542145693,2.5942243542145693,2.5942243542145693,2.5942243542145693,2.5942243542145693,2.5942243542145693,2.5942243542145693,2.5961509971494343,2.5961509971494343,2.5961509971494343,2.5961509971494343,2.5961509971494343,2.5961509971494343,2.5961509971494343,2.5961509971494343,2.6,2.6,2.6,2.6,2.6,2.6,2.6,2.6,2.6,2.6,2.6,2.6,2.6019223662515376,2.6019223662515376,2.6019223662515376,2.6019223662515376,2.6019223662515376,2.6019223662515376,2.6019223662515376,2.6019223662515376,2.60768096208106,2.60768096208106,2.60768096208106,2.60768096208106,2.60768096208106,2.60768096208106,2.60768096208106,2.60768096208106,2.60768096208106,2.60768096208106,2.60768096208106,2.60768096208106,2.60768096208106,2.60768096208106,2.60768096208106,2.60768096208106,2.6172504656604803,2.6172504656604803,2.6172504656604803,2.6172504656604803,2.6172504656604803,2.6172504656604803,2.6172504656604803,2.6172504656604803,2.6172504656604803,2.6172504656604803,2.6172504656604803,2.6172504656604803,2.6172504656604803,2.6172504656604803,2.6172504656604803,2.6172504656604803,2.6248809496813377,2.6248809496813377,2.6248809496813377,2.6248809496813377,2.6248809496813377,2.6248809496813377,2.6248809496813377,2.6248809496813377,2.6248809496813377,2.6248809496813377,2.6248809496813377,2.6248809496813377,2.6248809496813377,2.6248809496813377,2.6248809496813377,2.6248809496813377,2.6305892875931813,2.6305892875931813,2.6305892875931813,2.6305892875931813,2.6305892875931813,2.6305892875931813,2.6305892875931813,2.6305892875931813,2.6400757564888173,2.6400757564888173,2.6400757564888173,2.6400757564888173,2.6400757564888173,2.6400757564888173,2.6400757564888173,2.6400757564888173,2.6400757564888173,2.6400757564888173,2.6400757564888173,2.6400757564888173,2.6400757564888173,2.6400757564888173,2.6400757564888173,2.6400757564888173,2.6419689627245813,2.6419689627245813,2.6419689627245813,2.6419689627245813,2.6419689627245813,2.6419689627245813,2.6419689627245813,2.6419689627245813,2.6476404589747453,2.6476404589747453,2.6476404589747453,2.6476404589747453,2.6476404589747453,2.6476404589747453,2.6476404589747453,2.6476404589747453,2.657066051117285,2.657066051117285,2.657066051117285,2.657066051117285,2.657066051117285,2.657066051117285,2.657066051117285,2.657066051117285,2.6627053911388696,2.6627053911388696,2.6627053911388696,2.6627053911388696,2.6627053911388696,2.6627053911388696,2.6627053911388696,2.6627053911388696,2.668332812825267,2.668332812825267,2.668332812825267,2.668332812825267,2.668332812825267,2.668332812825267,2.668332812825267,2.668332812825267,2.683281572999748,2.683281572999748,2.683281572999748,2.683281572999748,2.683281572999748,2.683281572999748,2.683281572999748,2.683281572999748,2.6870057685088806,2.6870057685088806,2.6870057685088806,2.6870057685088806,2.6907248094147422,2.6907248094147422,2.6907248094147422,2.6907248094147422,2.6907248094147422,2.6907248094147422,2.6907248094147422,2.6907248094147422,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.6925824035672523,2.7,2.7,2.7,2.7,2.701851217221259,2.701851217221259,2.701851217221259,2.701851217221259,2.701851217221259,2.701851217221259,2.701851217221259,2.701851217221259,2.701851217221259,2.701851217221259,2.701851217221259,2.701851217221259,2.701851217221259,2.701851217221259,2.701851217221259,2.701851217221259,2.707397274136177,2.707397274136177,2.707397274136177,2.707397274136177,2.707397274136177,2.707397274136177,2.707397274136177,2.707397274136177,2.716615541441225,2.716615541441225,2.716615541441225,2.716615541441225,2.716615541441225,2.716615541441225,2.716615541441225,2.716615541441225,2.720294101747089,2.720294101747089,2.720294101747089,2.720294101747089,2.720294101747089,2.720294101747089,2.720294101747089,2.720294101747089,2.720294101747089,2.720294101747089,2.720294101747089,2.720294101747089,2.720294101747089,2.720294101747089,2.720294101747089,2.720294101747089,2.7294688127912363,2.7294688127912363,2.7294688127912363,2.7294688127912363,2.7294688127912363,2.7294688127912363,2.7294688127912363,2.7294688127912363,2.7294688127912363,2.7294688127912363,2.7294688127912363,2.7294688127912363,2.7294688127912363,2.7294688127912363,2.7294688127912363,2.7294688127912363,2.7313000567495327,2.7313000567495327,2.7313000567495327,2.7313000567495327,2.7313000567495327,2.7313000567495327,2.7313000567495327,2.7313000567495327,2.7459060435491964,2.7459060435491964,2.7459060435491964,2.7459060435491964,2.7459060435491964,2.7459060435491964,2.7459060435491964,2.7459060435491964,2.7459060435491964,2.7459060435491964,2.7459060435491964,2.7459060435491964,2.7459060435491964,2.7459060435491964,2.7459060435491964,2.7459060435491964,2.751363298439521,2.751363298439521,2.751363298439521,2.751363298439521,2.751363298439521,2.751363298439521,2.751363298439521,2.751363298439521,2.7586228448267445,2.7586228448267445,2.7586228448267445,2.7586228448267445,2.7586228448267445,2.7586228448267445,2.7586228448267445,2.7586228448267445,2.7658633371878665,2.7658633371878665,2.7658633371878665,2.7658633371878665,2.7658633371878665,2.7658633371878665,2.7658633371878665,2.7658633371878665,2.7658633371878665,2.7658633371878665,2.7658633371878665,2.7658633371878665,2.7658633371878665,2.7658633371878665,2.7658633371878665,2.7658633371878665,2.7730849247724096,2.7730849247724096,2.7730849247724096,2.7730849247724096,2.7730849247724096,2.7730849247724096,2.7730849247724096,2.7730849247724096,2.778488797889961,2.778488797889961,2.778488797889961,2.778488797889961,2.778488797889961,2.778488797889961,2.778488797889961,2.778488797889961,2.780287754891569,2.780287754891569,2.780287754891569,2.780287754891569,2.780287754891569,2.780287754891569,2.780287754891569,2.780287754891569,2.785677655436824,2.785677655436824,2.785677655436824,2.785677655436824,2.785677655436824,2.785677655436824,2.785677655436824,2.785677655436824,2.7892651361962706,2.7892651361962706,2.7892651361962706,2.7892651361962706,2.7892651361962706,2.7892651361962706,2.7892651361962706,2.7892651361962706,2.8000000000000003,2.8000000000000003,2.8000000000000003,2.8000000000000003,2.8017851452243803,2.8017851452243803,2.8017851452243803,2.8017851452243803,2.8017851452243803,2.8017851452243803,2.8017851452243803,2.8017851452243803,2.8017851452243803,2.8017851452243803,2.8017851452243803,2.8017851452243803,2.8017851452243803,2.8017851452243803,2.8017851452243803,2.8017851452243803,2.80713376952364,2.80713376952364,2.80713376952364,2.80713376952364,2.80713376952364,2.80713376952364,2.80713376952364,2.80713376952364,2.816025568065745,2.816025568065745,2.816025568065745,2.816025568065745,2.816025568065745,2.816025568065745,2.816025568065745,2.816025568065745,2.816025568065745,2.816025568065745,2.816025568065745,2.816025568065745,2.816025568065745,2.816025568065745,2.816025568065745,2.816025568065745,2.8178005607210745,2.8178005607210745,2.8178005607210745,2.8178005607210745,2.8178005607210745,2.8178005607210745,2.8178005607210745,2.8178005607210745,2.823118842698621,2.823118842698621,2.823118842698621,2.823118842698621,2.823118842698621,2.823118842698621,2.823118842698621,2.823118842698621,2.8284271247461903,2.8284271247461903,2.8284271247461903,2.8284271247461903,2.8284271247461903,2.8284271247461903,2.8284271247461903,2.8284271247461903,2.8284271247461903,2.8284271247461903,2.8284271247461903,2.8284271247461903,2.8301943396169813,2.8301943396169813,2.8301943396169813,2.8301943396169813,2.8301943396169813,2.8301943396169813,2.8301943396169813,2.8301943396169813,2.8319604517012595,2.8319604517012595,2.8319604517012595,2.8319604517012595,2.8319604517012595,2.8319604517012595,2.8319604517012595,2.8319604517012595,2.8425340807103794,2.8425340807103794,2.8425340807103794,2.8425340807103794,2.8425340807103794,2.8425340807103794,2.8425340807103794,2.8425340807103794,2.8442925306655784,2.8442925306655784,2.8442925306655784,2.8442925306655784,2.8442925306655784,2.8442925306655784,2.8442925306655784,2.8442925306655784,2.8460498941515415,2.8460498941515415,2.8460498941515415,2.8460498941515415,2.8460498941515415,2.8460498941515415,2.8460498941515415,2.8460498941515415,2.8600699292150185,2.8600699292150185,2.8600699292150185,2.8600699292150185,2.8600699292150185,2.8600699292150185,2.8600699292150185,2.8600699292150185,2.863564212655271,2.863564212655271,2.863564212655271,2.863564212655271,2.863564212655271,2.863564212655271,2.863564212655271,2.863564212655271,2.863564212655271,2.863564212655271,2.863564212655271,2.863564212655271,2.863564212655271,2.863564212655271,2.863564212655271,2.863564212655271,2.8653097563788807,2.8653097563788807,2.8653097563788807,2.8653097563788807,2.8653097563788807,2.8653097563788807,2.8653097563788807,2.8653097563788807,2.879236009777594,2.879236009777594,2.879236009777594,2.879236009777594,2.879236009777594,2.879236009777594,2.879236009777594,2.879236009777594,2.8844410203711917,2.8844410203711917,2.8844410203711917,2.8844410203711917,2.8844410203711917,2.8844410203711917,2.8844410203711917,2.8844410203711917,2.8861739379323623,2.8861739379323623,2.8861739379323623,2.8861739379323623,2.8861739379323623,2.8861739379323623,2.8861739379323623,2.8861739379323623,2.9000000000000004,2.9000000000000004,2.9000000000000004,2.9000000000000004,2.9000000000000004,2.9000000000000004,2.9000000000000004,2.9000000000000004,2.9000000000000004,2.9000000000000004,2.9000000000000004,2.9000000000000004,2.9017236257093817,2.9017236257093817,2.9017236257093817,2.9017236257093817,2.9017236257093817,2.9017236257093817,2.9017236257093817,2.9017236257093817,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.906888370749727,2.9120439557122073,2.9120439557122073,2.9120439557122073,2.9120439557122073,2.9120439557122073,2.9120439557122073,2.9120439557122073,2.9120439557122073,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.9154759474226504,2.920616373302047,2.920616373302047,2.920616373302047,2.920616373302047,2.920616373302047,2.920616373302047,2.920616373302047,2.920616373302047,2.9274562336608896,2.9274562336608896,2.9274562336608896,2.9274562336608896,2.9274562336608896,2.9274562336608896,2.9274562336608896,2.9274562336608896,2.9410882339705484,2.9410882339705484,2.9410882339705484,2.9410882339705484,2.9410882339705484,2.9410882339705484,2.9410882339705484,2.9410882339705484,2.9410882339705484,2.9410882339705484,2.9410882339705484,2.9410882339705484,2.9410882339705484,2.9410882339705484,2.9410882339705484,2.9410882339705484,2.9427877939124323,2.9427877939124323,2.9427877939124323,2.9427877939124323,2.9427877939124323,2.9427877939124323,2.9427877939124323,2.9427877939124323,2.95296461204668,2.95296461204668,2.95296461204668,2.95296461204668,2.95296461204668,2.95296461204668,2.95296461204668,2.95296461204668,2.954657340538832,2.954657340538832,2.954657340538832,2.954657340538832,2.954657340538832,2.954657340538832,2.954657340538832,2.954657340538832,2.96141857899217,2.96141857899217,2.96141857899217,2.96141857899217,2.96141857899217,2.96141857899217,2.96141857899217,2.96141857899217,2.968164415931166,2.968164415931166,2.968164415931166,2.968164415931166,2.968164415931166,2.968164415931166,2.968164415931166,2.968164415931166,2.9698484809834995,2.9698484809834995,2.9698484809834995,2.9698484809834995,2.973213749463701,2.973213749463701,2.973213749463701,2.973213749463701,2.973213749463701,2.973213749463701,2.973213749463701,2.973213749463701,2.973213749463701,2.973213749463701,2.973213749463701,2.973213749463701,2.973213749463701,2.973213749463701,2.973213749463701,2.973213749463701,2.98328677803526,2.98328677803526,2.98328677803526,2.98328677803526,2.98328677803526,2.98328677803526,2.98328677803526,2.98328677803526,2.98328677803526,2.98328677803526,2.98328677803526,2.98328677803526,2.98328677803526,2.98328677803526,2.98328677803526,2.98328677803526,2.9966648127543394,2.9966648127543394,2.9966648127543394,2.9966648127543394,2.9966648127543394,2.9966648127543394,2.9966648127543394,2.9966648127543394,3.0,3.0,3.0,3.0,3.0,3.0,3.0,3.0,3.0,3.0,3.0,3.0,3.001666203960727,3.001666203960727,3.001666203960727,3.001666203960727,3.001666203960727,3.001666203960727,3.001666203960727,3.001666203960727,3.001666203960727,3.001666203960727,3.001666203960727,3.001666203960727,3.001666203960727,3.001666203960727,3.001666203960727,3.001666203960727,3.006659275674582,3.006659275674582,3.006659275674582,3.006659275674582,3.006659275674582,3.006659275674582,3.006659275674582,3.006659275674582,3.0083217912982647,3.0083217912982647,3.0083217912982647,3.0083217912982647,3.0083217912982647,3.0083217912982647,3.0083217912982647,3.0083217912982647,3.0083217912982647,3.0083217912982647,3.0083217912982647,3.0083217912982647,3.0083217912982647,3.0083217912982647,3.0083217912982647,3.0083217912982647,3.014962686336267,3.014962686336267,3.014962686336267,3.014962686336267,3.014962686336267,3.014962686336267,3.014962686336267,3.014962686336267,3.023243291566195,3.023243291566195,3.023243291566195,3.023243291566195,3.023243291566195,3.023243291566195,3.023243291566195,3.023243291566195,3.0265491900843116,3.0265491900843116,3.0265491900843116,3.0265491900843116,3.0265491900843116,3.0265491900843116,3.0265491900843116,3.0265491900843116,3.036445290137795,3.036445290137795,3.036445290137795,3.036445290137795,3.036445290137795,3.036445290137795,3.036445290137795,3.036445290137795,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.04138126514911,3.0463092423455636,3.0463092423455636,3.0463092423455636,3.0463092423455636,3.0463092423455636,3.0463092423455636,3.0463092423455636,3.0463092423455636,3.0479501308256345,3.0479501308256345,3.0479501308256345,3.0479501308256345,3.0479501308256345,3.0479501308256345,3.0479501308256345,3.0479501308256345,3.0528675044947495,3.0528675044947495,3.0528675044947495,3.0528675044947495,3.0528675044947495,3.0528675044947495,3.0528675044947495,3.0528675044947495,3.0594117081556713,3.0594117081556713,3.0594117081556713,3.0594117081556713,3.0594117081556713,3.0594117081556713,3.0594117081556713,3.0594117081556713,3.0610455730027937,3.0610455730027937,3.0610455730027937,3.0610455730027937,3.0610455730027937,3.0610455730027937,3.0610455730027937,3.0610455730027937,3.0675723300355937,3.0675723300355937,3.0675723300355937,3.0675723300355937,3.0675723300355937,3.0675723300355937,3.0675723300355937,3.0675723300355937,3.0805843601498726,3.0805843601498726,3.0805843601498726,3.0805843601498726,3.0805843601498726,3.0805843601498726,3.0805843601498726,3.0805843601498726,3.0805843601498726,3.0805843601498726,3.0805843601498726,3.0805843601498726,3.0805843601498726,3.0805843601498726,3.0805843601498726,3.0805843601498726,3.0870698080866266,3.0870698080866266,3.0870698080866266,3.0870698080866266,3.0870698080866266,3.0870698080866266,3.0870698080866266,3.0870698080866266,3.0886890422961004,3.0886890422961004,3.0886890422961004,3.0886890422961004,3.0886890422961004,3.0886890422961004,3.0886890422961004,3.0886890422961004,3.1,3.1,3.1,3.1,3.101612483854165,3.101612483854165,3.101612483854165,3.101612483854165,3.101612483854165,3.101612483854165,3.101612483854165,3.101612483854165,3.101612483854165,3.101612483854165,3.101612483854165,3.101612483854165,3.101612483854165,3.101612483854165,3.101612483854165,3.101612483854165,3.104834939252005,3.104834939252005,3.104834939252005,3.104834939252005,3.104834939252005,3.104834939252005,3.104834939252005,3.104834939252005,3.1064449134018135,3.1064449134018135,3.1064449134018135,3.1064449134018135,3.1064449134018135,3.1064449134018135,3.1064449134018135,3.1064449134018135,3.1064449134018135,3.1064449134018135,3.1064449134018135,3.1064449134018135,3.1064449134018135,3.1064449134018135,3.1064449134018135,3.1064449134018135,3.111269837220809,3.111269837220809,3.111269837220809,3.111269837220809,3.1144823004794873,3.1144823004794873,3.1144823004794873,3.1144823004794873,3.1144823004794873,3.1144823004794873,3.1144823004794873,3.1144823004794873,3.1144823004794873,3.1144823004794873,3.1144823004794873,3.1144823004794873,3.1144823004794873,3.1144823004794873,3.1144823004794873,3.1144823004794873,3.1240998703626617,3.1240998703626617,3.1240998703626617,3.1240998703626617,3.1240998703626617,3.1240998703626617,3.1240998703626617,3.1240998703626617,3.125699921617557,3.125699921617557,3.125699921617557,3.125699921617557,3.125699921617557,3.125699921617557,3.125699921617557,3.125699921617557,3.130495168499706,3.130495168499706,3.130495168499706,3.130495168499706,3.130495168499706,3.130495168499706,3.130495168499706,3.130495168499706,3.132091952673165,3.132091952673165,3.132091952673165,3.132091952673165,3.132091952673165,3.132091952673165,3.132091952673165,3.132091952673165,3.1384709652950433,3.1384709652950433,3.1384709652950433,3.1384709652950433,3.1384709652950433,3.1384709652950433,3.1384709652950433,3.1384709652950433,3.1384709652950433,3.1384709652950433,3.1384709652950433,3.1384709652950433,3.1384709652950433,3.1384709652950433,3.1384709652950433,3.1384709652950433,3.1400636936215167,3.1400636936215167,3.1400636936215167,3.1400636936215167,3.1400636936215167,3.1400636936215167,3.1400636936215167,3.1400636936215167,3.1400636936215167,3.1400636936215167,3.1400636936215167,3.1400636936215167,3.1400636936215167,3.1400636936215167,3.1400636936215167,3.1400636936215167,3.157530680769389,3.157530680769389,3.157530680769389,3.157530680769389,3.157530680769389,3.157530680769389,3.157530680769389,3.157530680769389,3.1622776601683795,3.1622776601683795,3.1622776601683795,3.1622776601683795,3.1622776601683795,3.1622776601683795,3.1622776601683795,3.1622776601683795,3.1622776601683795,3.1622776601683795,3.1622776601683795,3.1622776601683795,3.1622776601683795,3.1622776601683795,3.1622776601683795,3.1622776601683795,3.176476034853718,3.176476034853718,3.176476034853718,3.176476034853718,3.176476034853718,3.176476034853718,3.176476034853718,3.176476034853718,3.178049716414141,3.178049716414141,3.178049716414141,3.178049716414141,3.178049716414141,3.178049716414141,3.178049716414141,3.178049716414141,3.178049716414141,3.178049716414141,3.178049716414141,3.178049716414141,3.178049716414141,3.178049716414141,3.178049716414141,3.178049716414141,3.1827660925679098,3.1827660925679098,3.1827660925679098,3.1827660925679098,3.1827660925679098,3.1827660925679098,3.1827660925679098,3.1827660925679098,3.1890437438203953,3.1890437438203953,3.1890437438203953,3.1890437438203953,3.1890437438203953,3.1890437438203953,3.1890437438203953,3.1890437438203953,3.1906112267087634,3.1906112267087634,3.1906112267087634,3.1906112267087634,3.1906112267087634,3.1906112267087634,3.1906112267087634,3.1906112267087634,3.1953090617340916,3.1953090617340916,3.1953090617340916,3.1953090617340916,3.1953090617340916,3.1953090617340916,3.1953090617340916,3.1953090617340916,3.2,3.2,3.2,3.2,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2015621187164243,3.2062439083762797,3.2062439083762797,3.2062439083762797,3.2062439083762797,3.2062439083762797,3.2062439083762797,3.2062439083762797,3.2062439083762797,3.2140317359976396,3.2140317359976396,3.2140317359976396,3.2140317359976396,3.2140317359976396,3.2140317359976396,3.2140317359976396,3.2140317359976396,3.2202484376209237,3.2202484376209237,3.2202484376209237,3.2202484376209237,3.2202484376209237,3.2202484376209237,3.2202484376209237,3.2202484376209237,3.2202484376209237,3.2202484376209237,3.2202484376209237,3.2202484376209237,3.2202484376209237,3.2202484376209237,3.2202484376209237,3.2202484376209237,3.2249030993194197,3.2249030993194197,3.2249030993194197,3.2249030993194197,3.2249030993194197,3.2249030993194197,3.2249030993194197,3.2249030993194197,3.2249030993194197,3.2249030993194197,3.2249030993194197,3.2249030993194197,3.2249030993194197,3.2249030993194197,3.2249030993194197,3.2249030993194197,3.228002478313795,3.228002478313795,3.228002478313795,3.228002478313795,3.228002478313795,3.228002478313795,3.228002478313795,3.228002478313795,3.2310988842807022,3.2310988842807022,3.2310988842807022,3.2310988842807022,3.2310988842807022,3.2310988842807022,3.2310988842807022,3.2310988842807022,3.2388269481403293,3.2388269481403293,3.2388269481403293,3.2388269481403293,3.2388269481403293,3.2388269481403293,3.2388269481403293,3.2388269481403293,3.2449961479175906,3.2449961479175906,3.2449961479175906,3.2449961479175906,3.2449961479175906,3.2449961479175906,3.2449961479175906,3.2449961479175906,3.252691193458119,3.252691193458119,3.252691193458119,3.252691193458119,3.2557641192199416,3.2557641192199416,3.2557641192199416,3.2557641192199416,3.2557641192199416,3.2557641192199416,3.2557641192199416,3.2557641192199416,3.2557641192199416,3.2557641192199416,3.2557641192199416,3.2557641192199416,3.2557641192199416,3.2557641192199416,3.2557641192199416,3.2557641192199416,3.257299494980466,3.257299494980466,3.257299494980466,3.257299494980466,3.257299494980466,3.257299494980466,3.257299494980466,3.257299494980466,3.264965543462902,3.264965543462902,3.264965543462902,3.264965543462902,3.264965543462902,3.264965543462902,3.264965543462902,3.264965543462902,3.264965543462902,3.264965543462902,3.264965543462902,3.264965543462902,3.264965543462902,3.264965543462902,3.264965543462902,3.264965543462902,3.2695565448543635,3.2695565448543635,3.2695565448543635,3.2695565448543635,3.2695565448543635,3.2695565448543635,3.2695565448543635,3.2695565448543635,3.27566787083184,3.27566787083184,3.27566787083184,3.27566787083184,3.27566787083184,3.27566787083184,3.27566787083184,3.27566787083184,3.27566787083184,3.27566787083184,3.27566787083184,3.27566787083184,3.27566787083184,3.27566787083184,3.27566787083184,3.27566787083184,3.2802438933713454,3.2802438933713454,3.2802438933713454,3.2802438933713454,3.2802438933713454,3.2802438933713454,3.2802438933713454,3.2802438933713454,3.2893768406797053,3.2893768406797053,3.2893768406797053,3.2893768406797053,3.2893768406797053,3.2893768406797053,3.2893768406797053,3.2893768406797053,3.2984845004941286,3.2984845004941286,3.2984845004941286,3.2984845004941286,3.2984845004941286,3.2984845004941286,3.2984845004941286,3.2984845004941286,3.3000000000000003,3.3000000000000003,3.3000000000000003,3.3000000000000003,3.3015148038438356,3.3015148038438356,3.3015148038438356,3.3015148038438356,3.3015148038438356,3.3015148038438356,3.3015148038438356,3.3015148038438356,3.3015148038438356,3.3015148038438356,3.3015148038438356,3.3015148038438356,3.3015148038438356,3.3015148038438356,3.3015148038438356,3.3015148038438356,3.3060550509633084,3.3060550509633084,3.3060550509633084,3.3060550509633084,3.3060550509633084,3.3060550509633084,3.3060550509633084,3.3060550509633084,3.3105890714493698,3.3105890714493698,3.3105890714493698,3.3105890714493698,3.3105890714493698,3.3105890714493698,3.3105890714493698,3.3105890714493698,3.3120990323358392,3.3120990323358392,3.3120990323358392,3.3120990323358392,3.3120990323358392,3.3120990323358392,3.3120990323358392,3.3120990323358392,3.3136083051561784,3.3136083051561784,3.3136083051561784,3.3136083051561784,3.3136083051561784,3.3136083051561784,3.3136083051561784,3.3136083051561784,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3241540277189325,3.3286633954186478,3.3286633954186478,3.3286633954186478,3.3286633954186478,3.3286633954186478,3.3286633954186478,3.3286633954186478,3.3286633954186478,3.330165161069343,3.330165161069343,3.330165161069343,3.330165161069343,3.330165161069343,3.330165161069343,3.330165161069343,3.330165161069343,3.3376638536557275,3.3376638536557275,3.3376638536557275,3.3376638536557275,3.3376638536557275,3.3376638536557275,3.3376638536557275,3.3376638536557275,3.342154993413681,3.342154993413681,3.342154993413681,3.342154993413681,3.342154993413681,3.342154993413681,3.342154993413681,3.342154993413681,3.3526109228480423,3.3526109228480423,3.3526109228480423,3.3526109228480423,3.3526109228480423,3.3526109228480423,3.3526109228480423,3.3526109228480423,3.3541019662496847,3.3541019662496847,3.3541019662496847,3.3541019662496847,3.3541019662496847,3.3541019662496847,3.3541019662496847,3.3541019662496847,3.3541019662496847,3.3541019662496847,3.3541019662496847,3.3541019662496847,3.3541019662496847,3.3541019662496847,3.3541019662496847,3.3541019662496847,3.3600595232822883,3.3600595232822883,3.3600595232822883,3.3600595232822883,3.3600595232822883,3.3600595232822883,3.3600595232822883,3.3600595232822883,3.361547262794322,3.361547262794322,3.361547262794322,3.361547262794322,3.361547262794322,3.361547262794322,3.361547262794322,3.361547262794322,3.361547262794322,3.361547262794322,3.361547262794322,3.361547262794322,3.361547262794322,3.361547262794322,3.361547262794322,3.361547262794322,3.3734255586866,3.3734255586866,3.3734255586866,3.3734255586866,3.3734255586866,3.3734255586866,3.3734255586866,3.3734255586866,3.3837848631377265,3.3837848631377265,3.3837848631377265,3.3837848631377265,3.3837848631377265,3.3837848631377265,3.3837848631377265,3.3837848631377265,3.3837848631377265,3.3837848631377265,3.3837848631377265,3.3837848631377265,3.3837848631377265,3.3837848631377265,3.3837848631377265,3.3837848631377265,3.394112549695428,3.394112549695428,3.394112549695428,3.394112549695428,3.3955853692699294,3.3955853692699294,3.3955853692699294,3.3955853692699294,3.3955853692699294,3.3955853692699294,3.3955853692699294,3.3955853692699294,3.3970575502926064,3.3970575502926064,3.3970575502926064,3.3970575502926064,3.3970575502926064,3.3970575502926064,3.3970575502926064,3.3970575502926064,3.4000000000000004,3.4000000000000004,3.4000000000000004,3.4000000000000004,3.4000000000000004,3.4000000000000004,3.4000000000000004,3.4000000000000004,3.4000000000000004,3.4000000000000004,3.4000000000000004,3.4000000000000004,3.40147027033899,3.40147027033899,3.40147027033899,3.40147027033899,3.40147027033899,3.40147027033899,3.40147027033899,3.40147027033899,3.40147027033899,3.40147027033899,3.40147027033899,3.40147027033899,3.40147027033899,3.40147027033899,3.40147027033899,3.40147027033899,3.405877273185281,3.405877273185281,3.405877273185281,3.405877273185281,3.405877273185281,3.405877273185281,3.405877273185281,3.405877273185281,3.405877273185281,3.405877273185281,3.405877273185281,3.405877273185281,3.405877273185281,3.405877273185281,3.405877273185281,3.405877273185281,3.4132096331752027,3.4132096331752027,3.4132096331752027,3.4132096331752027,3.4132096331752027,3.4132096331752027,3.4132096331752027,3.4132096331752027,3.4132096331752027,3.4132096331752027,3.4132096331752027,3.4132096331752027,3.4132096331752027,3.4132096331752027,3.4132096331752027,3.4132096331752027,3.4176014981270124,3.4176014981270124,3.4176014981270124,3.4176014981270124,3.4176014981270124,3.4176014981270124,3.4176014981270124,3.4176014981270124,3.4205262752974144,3.4205262752974144,3.4205262752974144,3.4205262752974144,3.4205262752974144,3.4205262752974144,3.4205262752974144,3.4205262752974144,3.4205262752974144,3.4205262752974144,3.4205262752974144,3.4205262752974144,3.4205262752974144,3.4205262752974144,3.4205262752974144,3.4205262752974144,3.423448553724738,3.423448553724738,3.423448553724738,3.423448553724738,3.423448553724738,3.423448553724738,3.423448553724738,3.423448553724738,3.4365680554879163,3.4365680554879163,3.4365680554879163,3.4365680554879163,3.4365680554879163,3.4365680554879163,3.4365680554879163,3.4365680554879163,3.440930106817051,3.440930106817051,3.440930106817051,3.440930106817051,3.440930106817051,3.440930106817051,3.440930106817051,3.440930106817051,3.4438350715445125,3.4438350715445125,3.4438350715445125,3.4438350715445125,3.4438350715445125,3.4438350715445125,3.4438350715445125,3.4438350715445125,3.4481879299133333,3.4481879299133333,3.4481879299133333,3.4481879299133333,3.4481879299133333,3.4481879299133333,3.4481879299133333,3.4481879299133333,3.4481879299133333,3.4481879299133333,3.4481879299133333,3.4481879299133333,3.4481879299133333,3.4481879299133333,3.4481879299133333,3.4481879299133333,3.4525353003264136,3.4525353003264136,3.4525353003264136,3.4525353003264136,3.4525353003264136,3.4525353003264136,3.4525353003264136,3.4525353003264136,3.4539832078341086,3.4539832078341086,3.4539832078341086,3.4539832078341086,3.4539832078341086,3.4539832078341086,3.4539832078341086,3.4539832078341086,3.465544690232692,3.465544690232692,3.465544690232692,3.465544690232692,3.465544690232692,3.465544690232692,3.465544690232692,3.465544690232692,3.4669871646719432,3.4669871646719432,3.4669871646719432,3.4669871646719432,3.4669871646719432,3.4669871646719432,3.4669871646719432,3.4669871646719432,3.471310991541957,3.471310991541957,3.471310991541957,3.471310991541957,3.471310991541957,3.471310991541957,3.471310991541957,3.471310991541957,3.471310991541957,3.471310991541957,3.471310991541957,3.471310991541957,3.471310991541957,3.471310991541957,3.471310991541957,3.471310991541957,3.4785054261852175,3.4785054261852175,3.4785054261852175,3.4785054261852175,3.4785054261852175,3.4785054261852175,3.4785054261852175,3.4785054261852175,3.4828149534536,3.4828149534536,3.4828149534536,3.4828149534536,3.4828149534536,3.4828149534536,3.4828149534536,3.4828149534536,3.488552708502482,3.488552708502482,3.488552708502482,3.488552708502482,3.488552708502482,3.488552708502482,3.488552708502482,3.488552708502482,3.492849839314596,3.492849839314596,3.492849839314596,3.492849839314596,3.492849839314596,3.492849839314596,3.492849839314596,3.492849839314596,3.492849839314596,3.492849839314596,3.492849839314596,3.492849839314596,3.492849839314596,3.492849839314596,3.492849839314596,3.492849839314596,3.4985711369071804,3.4985711369071804,3.4985711369071804,3.4985711369071804,3.4985711369071804,3.4985711369071804,3.4985711369071804,3.4985711369071804,3.5,3.5,3.5,3.5,3.5,3.5,3.5,3.5,3.5,3.5,3.5,3.5,3.5014282800023198,3.5014282800023198,3.5014282800023198,3.5014282800023198,3.5014282800023198,3.5014282800023198,3.5014282800023198,3.5014282800023198,3.505709628591621,3.505709628591621,3.505709628591621,3.505709628591621,3.505709628591621,3.505709628591621,3.505709628591621,3.505709628591621,3.511409973215888,3.511409973215888,3.511409973215888,3.511409973215888,3.511409973215888,3.511409973215888,3.511409973215888,3.511409973215888,3.5128336140500593,3.5128336140500593,3.5128336140500593,3.5128336140500593,3.5128336140500593,3.5128336140500593,3.5128336140500593,3.5128336140500593,3.517101079013795,3.517101079013795,3.517101079013795,3.517101079013795,3.517101079013795,3.517101079013795,3.517101079013795,3.517101079013795,3.522782990761707,3.522782990761707,3.522782990761707,3.522782990761707,3.522782990761707,3.522782990761707,3.522782990761707,3.522782990761707,3.522782990761707,3.522782990761707,3.522782990761707,3.522782990761707,3.522782990761707,3.522782990761707,3.522782990761707,3.522782990761707,3.5341194094144583,3.5341194094144583,3.5341194094144583,3.5341194094144583,3.5341194094144583,3.5341194094144583,3.5341194094144583,3.5341194094144583,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5355339059327378,3.5383612025908264,3.5383612025908264,3.5383612025908264,3.5383612025908264,3.5383612025908264,3.5383612025908264,3.5383612025908264,3.5383612025908264,3.54400902933387,3.54400902933387,3.54400902933387,3.54400902933387,3.54400902933387,3.54400902933387,3.54400902933387,3.54400902933387,3.54682957019364,3.54682957019364,3.54682957019364,3.54682957019364,3.54682957019364,3.54682957019364,3.54682957019364,3.54682957019364,3.54682957019364,3.54682957019364,3.54682957019364,3.54682957019364,3.54682957019364,3.54682957019364,3.54682957019364,3.54682957019364,3.5510561809129406,3.5510561809129406,3.5510561809129406,3.5510561809129406,3.5510561809129406,3.5510561809129406,3.5510561809129406,3.5510561809129406,3.5510561809129406,3.5510561809129406,3.5510561809129406,3.5510561809129406,3.5510561809129406,3.5510561809129406,3.5510561809129406,3.5510561809129406,3.560898762952972,3.560898762952972,3.560898762952972,3.560898762952972,3.560898762952972,3.560898762952972,3.560898762952972,3.560898762952972,3.56931365951495,3.56931365951495,3.56931365951495,3.56931365951495,3.56931365951495,3.56931365951495,3.56931365951495,3.56931365951495,3.5735136770411273,3.5735136770411273,3.5735136770411273,3.5735136770411273,3.5735136770411273,3.5735136770411273,3.5735136770411273,3.5735136770411273,3.577708763999664,3.577708763999664,3.577708763999664,3.577708763999664,3.577708763999664,3.577708763999664,3.577708763999664,3.577708763999664,3.5805027579936315,3.5805027579936315,3.5805027579936315,3.5805027579936315,3.5805027579936315,3.5805027579936315,3.5805027579936315,3.5805027579936315,3.5846896657869842,3.5846896657869842,3.5846896657869842,3.5846896657869842,3.5846896657869842,3.5846896657869842,3.5846896657869842,3.5846896657869842,3.5846896657869842,3.5846896657869842,3.5846896657869842,3.5846896657869842,3.5846896657869842,3.5846896657869842,3.5846896657869842,3.5846896657869842,3.590264614203248,3.590264614203248,3.590264614203248,3.590264614203248,3.590264614203248,3.590264614203248,3.590264614203248,3.590264614203248,3.6,3.6,3.6,3.6,3.6013886210738217,3.6013886210738217,3.6013886210738217,3.6013886210738217,3.6013886210738217,3.6013886210738217,3.6013886210738217,3.6013886210738217,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6055512754639896,3.6069377593742864,3.6069377593742864,3.6069377593742864,3.6069377593742864,3.6069377593742864,3.6069377593742864,3.6069377593742864,3.6069377593742864,3.6124783736376886,3.6124783736376886,3.6124783736376886,3.6124783736376886,3.6124783736376886,3.6124783736376886,3.6124783736376886,3.6124783736376886,3.6124783736376886,3.6124783736376886,3.6124783736376886,3.6124783736376886,3.6124783736376886,3.6124783736376886,3.6124783736376886,3.6124783736376886,3.6138621999185307,3.6138621999185307,3.6138621999185307,3.6138621999185307,3.6138621999185307,3.6138621999185307,3.6138621999185307,3.6138621999185307,3.622154055254967,3.622154055254967,3.622154055254967,3.622154055254967,3.622154055254967,3.622154055254967,3.622154055254967,3.622154055254967,3.623534186398688,3.623534186398688,3.623534186398688,3.623534186398688,3.623534186398688,3.623534186398688,3.623534186398688,3.623534186398688,3.623534186398688,3.623534186398688,3.623534186398688,3.623534186398688,3.623534186398688,3.623534186398688,3.623534186398688,3.623534186398688,3.624913792078372,3.624913792078372,3.624913792078372,3.624913792078372,3.624913792078372,3.624913792078372,3.624913792078372,3.624913792078372,3.6345563690772495,3.6345563690772495,3.6345563690772495,3.6345563690772495,3.6345563690772495,3.6345563690772495,3.6345563690772495,3.6345563690772495,3.635931792539569,3.635931792539569,3.635931792539569,3.635931792539569,3.635931792539569,3.635931792539569,3.635931792539569,3.635931792539569,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.6400549446402595,3.649657518178932,3.649657518178932,3.649657518178932,3.649657518178932,3.649657518178932,3.649657518178932,3.649657518178932,3.649657518178932,3.6619666847201113,3.6619666847201113,3.6619666847201113,3.6619666847201113,3.6619666847201113,3.6619666847201113,3.6619666847201113,3.6619666847201113,3.66742416417845,3.66742416417845,3.66742416417845,3.66742416417845,3.66742416417845,3.66742416417845,3.66742416417845,3.66742416417845,3.66742416417845,3.66742416417845,3.66742416417845,3.66742416417845,3.66742416417845,3.66742416417845,3.66742416417845,3.66742416417845,3.6687872655688283,3.6687872655688283,3.6687872655688283,3.6687872655688283,3.6687872655688283,3.6687872655688283,3.6687872655688283,3.6687872655688283,3.6715119501371642,3.6715119501371642,3.6715119501371642,3.6715119501371642,3.6715119501371642,3.6715119501371642,3.6715119501371642,3.6715119501371642,3.676955262170047,3.676955262170047,3.676955262170047,3.676955262170047,3.676955262170047,3.676955262170047,3.676955262170047,3.676955262170047,3.676955262170047,3.676955262170047,3.676955262170047,3.676955262170047,3.6796738985948196,3.6796738985948196,3.6796738985948196,3.6796738985948196,3.6796738985948196,3.6796738985948196,3.6796738985948196,3.6796738985948196,3.687817782917155,3.687817782917155,3.687817782917155,3.687817782917155,3.687817782917155,3.687817782917155,3.687817782917155,3.687817782917155,3.687817782917155,3.687817782917155,3.687817782917155,3.687817782917155,3.687817782917155,3.687817782917155,3.687817782917155,3.687817782917155,3.6891733491393435,3.6891733491393435,3.6891733491393435,3.6891733491393435,3.6891733491393435,3.6891733491393435,3.6891733491393435,3.6891733491393435,3.7,3.7,3.7,3.7,3.7,3.7,3.7,3.7,3.7,3.7,3.7,3.7,3.7013511046643495,3.7013511046643495,3.7013511046643495,3.7013511046643495,3.7013511046643495,3.7013511046643495,3.7013511046643495,3.7013511046643495,3.7013511046643495,3.7013511046643495,3.7013511046643495,3.7013511046643495,3.7013511046643495,3.7013511046643495,3.7013511046643495,3.7013511046643495,3.7054014627297813,3.7054014627297813,3.7054014627297813,3.7054014627297813,3.7054014627297813,3.7054014627297813,3.7054014627297813,3.7054014627297813,3.710795063055895,3.710795063055895,3.710795063055895,3.710795063055895,3.710795063055895,3.710795063055895,3.710795063055895,3.710795063055895,3.712142238654117,3.712142238654117,3.712142238654117,3.712142238654117,3.712142238654117,3.712142238654117,3.712142238654117,3.712142238654117,3.712142238654117,3.712142238654117,3.712142238654117,3.712142238654117,3.712142238654117,3.712142238654117,3.712142238654117,3.712142238654117,3.716180835212409,3.716180835212409,3.716180835212409,3.716180835212409,3.716180835212409,3.716180835212409,3.716180835212409,3.716180835212409,3.720215047547655,3.720215047547655,3.720215047547655,3.720215047547655,3.720215047547655,3.720215047547655,3.720215047547655,3.720215047547655,3.721558813185679,3.721558813185679,3.721558813185679,3.721558813185679,3.721558813185679,3.721558813185679,3.721558813185679,3.721558813185679,3.721558813185679,3.721558813185679,3.721558813185679,3.721558813185679,3.721558813185679,3.721558813185679,3.721558813185679,3.721558813185679,3.7336309405188945,3.7336309405188945,3.7336309405188945,3.7336309405188945,3.7336309405188945,3.7336309405188945,3.7336309405188945,3.7336309405188945,3.7336309405188945,3.7336309405188945,3.7336309405188945,3.7336309405188945,3.7336309405188945,3.7336309405188945,3.7336309405188945,3.7336309405188945,3.736308338453881,3.736308338453881,3.736308338453881,3.736308338453881,3.736308338453881,3.736308338453881,3.736308338453881,3.736308338453881,3.7443290453698115,3.7443290453698115,3.7443290453698115,3.7443290453698115,3.7443290453698115,3.7443290453698115,3.7443290453698115,3.7443290453698115,3.748332962798263,3.748332962798263,3.748332962798263,3.748332962798263,3.748332962798263,3.748332962798263,3.748332962798263,3.748332962798263,3.748332962798263,3.748332962798263,3.748332962798263,3.748332962798263,3.748332962798263,3.748332962798263,3.748332962798263,3.748332962798263,3.753664875824692,3.753664875824692,3.753664875824692,3.753664875824692,3.753664875824692,3.753664875824692,3.753664875824692,3.753664875824692,3.7576588456111875,3.7576588456111875,3.7576588456111875,3.7576588456111875,3.7576588456111875,3.7576588456111875,3.7576588456111875,3.7576588456111875,3.7589892258425004,3.7589892258425004,3.7589892258425004,3.7589892258425004,3.7589892258425004,3.7589892258425004,3.7589892258425004,3.7589892258425004,3.7643060449437424,3.7643060449437424,3.7643060449437424,3.7643060449437424,3.7643060449437424,3.7643060449437424,3.7643060449437424,3.7643060449437424,3.7643060449437424,3.7643060449437424,3.7643060449437424,3.7643060449437424,3.7643060449437424,3.7643060449437424,3.7643060449437424,3.7643060449437424,3.7656340767525465,3.7656340767525465,3.7656340767525465,3.7656340767525465,3.7656340767525465,3.7656340767525465,3.7656340767525465,3.7656340767525465,3.769615364994153,3.769615364994153,3.769615364994153,3.769615364994153,3.769615364994153,3.769615364994153,3.769615364994153,3.769615364994153,3.7735924528226414,3.7735924528226414,3.7735924528226414,3.7735924528226414,3.7735924528226414,3.7735924528226414,3.7735924528226414,3.7735924528226414,3.78021163428716,3.78021163428716,3.78021163428716,3.78021163428716,3.78021163428716,3.78021163428716,3.78021163428716,3.78021163428716,3.78549864614954,3.78549864614954,3.78549864614954,3.78549864614954,3.78549864614954,3.78549864614954,3.78549864614954,3.78549864614954,3.7947331922020555,3.7947331922020555,3.7947331922020555,3.7947331922020555,3.7947331922020555,3.7947331922020555,3.7947331922020555,3.7947331922020555,3.8000000000000003,3.8000000000000003,3.8000000000000003,3.8000000000000003,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8013155617496426,3.8052595180880893,3.8052595180880893,3.8052595180880893,3.8052595180880893,3.8052595180880893,3.8052595180880893,3.8052595180880893,3.8052595180880893,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.8078865529319543,3.811823710509184,3.811823710509184,3.811823710509184,3.811823710509184,3.811823710509184,3.811823710509184,3.811823710509184,3.811823710509184,3.818376618407357,3.818376618407357,3.818376618407357,3.818376618407357,3.82099463490856,3.82099463490856,3.82099463490856,3.82099463490856,3.82099463490856,3.82099463490856,3.82099463490856,3.82099463490856,3.82099463490856,3.82099463490856,3.82099463490856,3.82099463490856,3.82099463490856,3.82099463490856,3.82099463490856,3.82099463490856,3.827531841800928,3.827531841800928,3.827531841800928,3.827531841800928,3.827531841800928,3.827531841800928,3.827531841800928,3.827531841800928,3.827531841800928,3.827531841800928,3.827531841800928,3.827531841800928,3.827531841800928,3.827531841800928,3.827531841800928,3.827531841800928,3.8288379438153295,3.8288379438153295,3.8288379438153295,3.8288379438153295,3.8288379438153295,3.8288379438153295,3.8288379438153295,3.8288379438153295,3.83275357934736,3.83275357934736,3.83275357934736,3.83275357934736,3.83275357934736,3.83275357934736,3.83275357934736,3.83275357934736,3.83275357934736,3.83275357934736,3.83275357934736,3.83275357934736,3.83275357934736,3.83275357934736,3.83275357934736,3.83275357934736,3.8418745424597094,3.8418745424597094,3.8418745424597094,3.8418745424597094,3.8418745424597094,3.8418745424597094,3.8418745424597094,3.8418745424597094,3.8470768123342687,3.8470768123342687,3.8470768123342687,3.8470768123342687,3.8470768123342687,3.8470768123342687,3.8470768123342687,3.8470768123342687,3.8470768123342687,3.8470768123342687,3.8470768123342687,3.8470768123342687,3.8470768123342687,3.8470768123342687,3.8470768123342687,3.8470768123342687,3.8483762809787714,3.8483762809787714,3.8483762809787714,3.8483762809787714,3.8483762809787714,3.8483762809787714,3.8483762809787714,3.8483762809787714,3.858756276314948,3.858756276314948,3.858756276314948,3.858756276314948,3.858756276314948,3.858756276314948,3.858756276314948,3.858756276314948,3.8600518131237567,3.8600518131237567,3.8600518131237567,3.8600518131237567,3.8600518131237567,3.8600518131237567,3.8600518131237567,3.8600518131237567,3.8600518131237567,3.8600518131237567,3.8600518131237567,3.8600518131237567,3.8600518131237567,3.8600518131237567,3.8600518131237567,3.8600518131237567,3.8626415831655936,3.8626415831655936,3.8626415831655936,3.8626415831655936,3.8626415831655936,3.8626415831655936,3.8626415831655936,3.8626415831655936,3.8639358172723313,3.8639358172723313,3.8639358172723313,3.8639358172723313,3.8639358172723313,3.8639358172723313,3.8639358172723313,3.8639358172723313,3.88329756778952,3.88329756778952,3.88329756778952,3.88329756778952,3.88329756778952,3.88329756778952,3.88329756778952,3.88329756778952,3.88329756778952,3.88329756778952,3.88329756778952,3.88329756778952,3.88329756778952,3.88329756778952,3.88329756778952,3.88329756778952,3.8897300677553446,3.8897300677553446,3.8897300677553446,3.8897300677553446,3.8897300677553446,3.8897300677553446,3.8897300677553446,3.8897300677553446,3.8897300677553446,3.8897300677553446,3.8897300677553446,3.8897300677553446,3.8897300677553446,3.8897300677553446,3.8897300677553446,3.8897300677553446,3.8910152916687437,3.8910152916687437,3.8910152916687437,3.8910152916687437,3.8910152916687437,3.8910152916687437,3.8910152916687437,3.8910152916687437,3.8948684188300895,3.8948684188300895,3.8948684188300895,3.8948684188300895,3.8948684188300895,3.8948684188300895,3.8948684188300895,3.8948684188300895,3.8948684188300895,3.8948684188300895,3.8948684188300895,3.8948684188300895,3.8948684188300895,3.8948684188300895,3.8948684188300895,3.8948684188300895,3.9000000000000004,3.9000000000000004,3.9000000000000004,3.9000000000000004,3.9000000000000004,3.9000000000000004,3.9000000000000004,3.9000000000000004,3.9000000000000004,3.9000000000000004,3.9000000000000004,3.9000000000000004,3.901281840626232,3.901281840626232,3.901281840626232,3.901281840626232,3.901281840626232,3.901281840626232,3.901281840626232,3.901281840626232,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.905124837953327,3.9115214431215892,3.9115214431215892,3.9115214431215892,3.9115214431215892,3.9115214431215892,3.9115214431215892,3.9115214431215892,3.9115214431215892,3.9115214431215892,3.9115214431215892,3.9115214431215892,3.9115214431215892,3.9115214431215892,3.9115214431215892,3.9115214431215892,3.9115214431215892,3.920459156782532,3.920459156782532,3.920459156782532,3.920459156782532,3.920459156782532,3.920459156782532,3.920459156782532,3.920459156782532,3.920459156782532,3.920459156782532,3.920459156782532,3.920459156782532,3.920459156782532,3.920459156782532,3.920459156782532,3.920459156782532,3.9217343102255158,3.9217343102255158,3.9217343102255158,3.9217343102255158,3.9217343102255158,3.9217343102255158,3.9217343102255158,3.9217343102255158,3.9293765408777,3.9293765408777,3.9293765408777,3.9293765408777,3.9293765408777,3.9293765408777,3.9293765408777,3.9293765408777,3.9319206502675,3.9319206502675,3.9319206502675,3.9319206502675,3.9319206502675,3.9319206502675,3.9319206502675,3.9319206502675,3.935733730830886,3.935733730830886,3.935733730830886,3.935733730830886,3.935733730830886,3.935733730830886,3.935733730830886,3.935733730830886,3.939543120718442,3.939543120718442,3.939543120718442,3.939543120718442,3.939543120718442,3.939543120718442,3.939543120718442,3.939543120718442,3.9408120990476068,3.9408120990476068,3.9408120990476068,3.9408120990476068,3.9408120990476068,3.9408120990476068,3.9408120990476068,3.9408120990476068,3.9446165846632044,3.9446165846632044,3.9446165846632044,3.9446165846632044,3.9446165846632044,3.9446165846632044,3.9446165846632044,3.9446165846632044,3.9458839313897722,3.9458839313897722,3.9458839313897722,3.9458839313897722,3.9458839313897722,3.9458839313897722,3.9458839313897722,3.9458839313897722,3.9560080889704965,3.9560080889704965,3.9560080889704965,3.9560080889704965,3.9560080889704965,3.9560080889704965,3.9560080889704965,3.9560080889704965,3.9560080889704965,3.9560080889704965,3.9560080889704965,3.9560080889704965,3.9560080889704965,3.9560080889704965,3.9560080889704965,3.9560080889704965,3.9597979746446663,3.9597979746446663,3.9597979746446663,3.9597979746446663,3.9623225512317903,3.9623225512317903,3.9623225512317903,3.9623225512317903,3.9623225512317903,3.9623225512317903,3.9623225512317903,3.9623225512317903,3.9623225512317903,3.9623225512317903,3.9623225512317903,3.9623225512317903,3.9623225512317903,3.9623225512317903,3.9623225512317903,3.9623225512317903,3.966106403010388,3.966106403010388,3.966106403010388,3.966106403010388,3.966106403010388,3.966106403010388,3.966106403010388,3.966106403010388,3.9698866482558417,3.9698866482558417,3.9698866482558417,3.9698866482558417,3.9698866482558417,3.9698866482558417,3.9698866482558417,3.9698866482558417,3.9812058474788765,3.9812058474788765,3.9812058474788765,3.9812058474788765,3.9812058474788765,3.9812058474788765,3.9812058474788765,3.9812058474788765,3.9812058474788765,3.9812058474788765,3.9812058474788765,3.9812058474788765,3.9812058474788765,3.9812058474788765,3.9812058474788765,3.9812058474788765,3.982461550347976,3.982461550347976,3.982461550347976,3.982461550347976,3.982461550347976,3.982461550347976,3.982461550347976,3.982461550347976,3.982461550347976,3.982461550347976,3.982461550347976,3.982461550347976,3.982461550347976,3.982461550347976,3.982461550347976,3.982461550347976,3.984971769034255,3.984971769034255,3.984971769034255,3.984971769034255,3.984971769034255,3.984971769034255,3.984971769034255,3.984971769034255,3.9924929555354263,3.9924929555354263,3.9924929555354263,3.9924929555354263,3.9924929555354263,3.9924929555354263,3.9924929555354263,3.9924929555354263,3.9962482405376174,3.9962482405376174,3.9962482405376174,3.9962482405376174,3.9962482405376174,3.9962482405376174,3.9962482405376174,3.9962482405376174,4.0,4.0,4.0,4.0,4.0,4.0,4.0,4.0,4.0,4.0,4.0,4.0,4.001249804748511,4.001249804748511,4.001249804748511,4.001249804748511,4.001249804748511,4.001249804748511,4.001249804748511,4.001249804748511,4.0024992192379,4.0024992192379,4.0024992192379,4.0024992192379,4.0024992192379,4.0024992192379,4.0024992192379,4.0024992192379,4.004996878900157,4.004996878900157,4.004996878900157,4.004996878900157,4.004996878900157,4.004996878900157,4.004996878900157,4.004996878900157,4.011234224026317,4.011234224026317,4.011234224026317,4.011234224026317,4.011234224026317,4.011234224026317,4.011234224026317,4.011234224026317,4.016217125604641,4.016217125604641,4.016217125604641,4.016217125604641,4.016217125604641,4.016217125604641,4.016217125604641,4.016217125604641,4.019950248448356,4.019950248448356,4.019950248448356,4.019950248448356,4.019950248448356,4.019950248448356,4.019950248448356,4.019950248448356,4.022437072223753,4.022437072223753,4.022437072223753,4.022437072223753,4.022437072223753,4.022437072223753,4.022437072223753,4.022437072223753,4.024922359499622,4.024922359499622,4.024922359499622,4.024922359499622,4.024922359499622,4.024922359499622,4.024922359499622,4.024922359499622,4.026164427839479,4.026164427839479,4.026164427839479,4.026164427839479,4.026164427839479,4.026164427839479,4.026164427839479,4.026164427839479,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.031128874149275,4.036087214122113,4.036087214122113,4.036087214122113,4.036087214122113,4.036087214122113,4.036087214122113,4.036087214122113,4.036087214122113,4.044749683231337,4.044749683231337,4.044749683231337,4.044749683231337,4.044749683231337,4.044749683231337,4.044749683231337,4.044749683231337,4.045985664828782,4.045985664828782,4.045985664828782,4.045985664828782,4.045985664828782,4.045985664828782,4.045985664828782,4.045985664828782,4.049691346263318,4.049691346263318,4.049691346263318,4.049691346263318,4.049691346263318,4.049691346263318,4.049691346263318,4.049691346263318,4.049691346263318,4.049691346263318,4.049691346263318,4.049691346263318,4.049691346263318,4.049691346263318,4.049691346263318,4.049691346263318,4.052159917870962,4.052159917870962,4.052159917870962,4.052159917870962,4.052159917870962,4.052159917870962,4.052159917870962,4.052159917870962,4.060788100849391,4.060788100849391,4.060788100849391,4.060788100849391,4.060788100849391,4.060788100849391,4.060788100849391,4.060788100849391,4.060788100849391,4.060788100849391,4.060788100849391,4.060788100849391,4.060788100849391,4.060788100849391,4.060788100849391,4.060788100849391,4.070626487409524,4.070626487409524,4.070626487409524,4.070626487409524,4.070626487409524,4.070626487409524,4.070626487409524,4.070626487409524,4.071854614300467,4.071854614300467,4.071854614300467,4.071854614300467,4.071854614300467,4.071854614300467,4.071854614300467,4.071854614300467,4.079215610874228,4.079215610874228,4.079215610874228,4.079215610874228,4.079215610874228,4.079215610874228,4.079215610874228,4.079215610874228,4.080441152620633,4.080441152620633,4.080441152620633,4.080441152620633,4.080441152620633,4.080441152620633,4.080441152620633,4.080441152620633,4.080441152620633,4.080441152620633,4.080441152620633,4.080441152620633,4.080441152620633,4.080441152620633,4.080441152620633,4.080441152620633,4.08166632639171,4.08166632639171,4.08166632639171,4.08166632639171,4.08166632639171,4.08166632639171,4.08166632639171,4.08166632639171,4.085339643163099,4.085339643163099,4.085339643163099,4.085339643163099,4.085339643163099,4.085339643163099,4.085339643163099,4.085339643163099,4.1000000000000005,4.1000000000000005,4.1000000000000005,4.1000000000000005,4.1000000000000005,4.1000000000000005,4.1000000000000005,4.1000000000000005,4.1000000000000005,4.1000000000000005,4.1000000000000005,4.1000000000000005,4.1012193308819755,4.1012193308819755,4.1012193308819755,4.1012193308819755,4.1012193308819755,4.1012193308819755,4.1012193308819755,4.1012193308819755,4.1012193308819755,4.1012193308819755,4.1012193308819755,4.1012193308819755,4.1036569057366385,4.1036569057366385,4.1036569057366385,4.1036569057366385,4.1036569057366385,4.1036569057366385,4.1036569057366385,4.1036569057366385,4.104875150354759,4.104875150354759,4.104875150354759,4.104875150354759,4.104875150354759,4.104875150354759,4.104875150354759,4.104875150354759,4.104875150354759,4.104875150354759,4.104875150354759,4.104875150354759,4.104875150354759,4.104875150354759,4.104875150354759,4.104875150354759,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.110960958218893,4.114608122288197,4.114608122288197,4.114608122288197,4.114608122288197,4.114608122288197,4.114608122288197,4.114608122288197,4.114608122288197,4.118252056394801,4.118252056394801,4.118252056394801,4.118252056394801,4.118252056394801,4.118252056394801,4.118252056394801,4.118252056394801,4.119465984809196,4.119465984809196,4.119465984809196,4.119465984809196,4.119465984809196,4.119465984809196,4.119465984809196,4.119465984809196,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.123105625617661,4.1303752856126765,4.1303752856126765,4.1303752856126765,4.1303752856126765,4.1303752856126765,4.1303752856126765,4.1303752856126765,4.1303752856126765,4.1340053217188775,4.1340053217188775,4.1340053217188775,4.1340053217188775,4.1340053217188775,4.1340053217188775,4.1340053217188775,4.1340053217188775,4.1400483088968905,4.1400483088968905,4.1400483088968905,4.1400483088968905,4.1400483088968905,4.1400483088968905,4.1400483088968905,4.1400483088968905,4.1436698710201325,4.1436698710201325,4.1436698710201325,4.1436698710201325,4.1436698710201325,4.1436698710201325,4.1436698710201325,4.1436698710201325,4.1436698710201325,4.1436698710201325,4.1436698710201325,4.1436698710201325,4.1436698710201325,4.1436698710201325,4.1436698710201325,4.1436698710201325,4.148493702538309,4.148493702538309,4.148493702538309,4.148493702538309,4.148493702538309,4.148493702538309,4.148493702538309,4.148493702538309,4.159326868617085,4.159326868617085,4.159326868617085,4.159326868617085,4.159326868617085,4.159326868617085,4.159326868617085,4.159326868617085,4.159326868617085,4.159326868617085,4.159326868617085,4.159326868617085,4.159326868617085,4.159326868617085,4.159326868617085,4.159326868617085,4.161730409336963,4.161730409336963,4.161730409336963,4.161730409336963,4.161730409336963,4.161730409336963,4.161730409336963,4.161730409336963,4.16293165929973,4.16293165929973,4.16293165929973,4.16293165929973,4.16293165929973,4.16293165929973,4.16293165929973,4.16293165929973,4.167733196834941,4.167733196834941,4.167733196834941,4.167733196834941,4.167733196834941,4.167733196834941,4.167733196834941,4.167733196834941,4.172529209005013,4.172529209005013,4.172529209005013,4.172529209005013,4.172529209005013,4.172529209005013,4.172529209005013,4.172529209005013,4.17612260356422,4.17612260356422,4.17612260356422,4.17612260356422,4.17612260356422,4.17612260356422,4.17612260356422,4.17612260356422,4.177319714841085,4.177319714841085,4.177319714841085,4.177319714841085,4.177319714841085,4.177319714841085,4.177319714841085,4.177319714841085,4.177319714841085,4.177319714841085,4.177319714841085,4.177319714841085,4.177319714841085,4.177319714841085,4.177319714841085,4.177319714841085,4.178516483155236,4.178516483155236,4.178516483155236,4.178516483155236,4.178516483155236,4.178516483155236,4.178516483155236,4.178516483155236,4.186884283091664,4.186884283091664,4.186884283091664,4.186884283091664,4.186884283091664,4.186884283091664,4.186884283091664,4.186884283091664,4.188078318274385,4.188078318274385,4.188078318274385,4.188078318274385,4.188078318274385,4.188078318274385,4.188078318274385,4.188078318274385,4.197618372363071,4.197618372363071,4.197618372363071,4.197618372363071,4.197618372363071,4.197618372363071,4.197618372363071,4.197618372363071,4.2,4.2,4.2,4.2,4.201190307520001,4.201190307520001,4.201190307520001,4.201190307520001,4.201190307520001,4.201190307520001,4.201190307520001,4.201190307520001,4.201190307520001,4.201190307520001,4.201190307520001,4.201190307520001,4.201190307520001,4.201190307520001,4.201190307520001,4.201190307520001,4.204759208325728,4.204759208325728,4.204759208325728,4.204759208325728,4.204759208325728,4.204759208325728,4.204759208325728,4.204759208325728,4.204759208325728,4.204759208325728,4.204759208325728,4.204759208325728,4.204759208325728,4.204759208325728,4.204759208325728,4.204759208325728,4.205948168962618,4.205948168962618,4.205948168962618,4.205948168962618,4.205948168962618,4.205948168962618,4.205948168962618,4.205948168962618,4.205948168962618,4.205948168962618,4.205948168962618,4.205948168962618,4.205948168962618,4.205948168962618,4.205948168962618,4.205948168962618,4.21070065428546,4.21070065428546,4.21070065428546,4.21070065428546,4.21070065428546,4.21070065428546,4.21070065428546,4.21070065428546,4.215447781671599,4.215447781671599,4.215447781671599,4.215447781671599,4.215447781671599,4.215447781671599,4.215447781671599,4.215447781671599,4.219004621945798,4.219004621945798,4.219004621945798,4.219004621945798,4.219004621945798,4.219004621945798,4.219004621945798,4.219004621945798,4.219004621945798,4.219004621945798,4.219004621945798,4.219004621945798,4.219004621945798,4.219004621945798,4.219004621945798,4.219004621945798,4.220189569201838,4.220189569201838,4.220189569201838,4.220189569201838,4.220189569201838,4.220189569201838,4.220189569201838,4.220189569201838,4.220189569201838,4.220189569201838,4.220189569201838,4.220189569201838,4.220189569201838,4.220189569201838,4.220189569201838,4.220189569201838,4.2296571965113205,4.2296571965113205,4.2296571965113205,4.2296571965113205,4.2296571965113205,4.2296571965113205,4.2296571965113205,4.2296571965113205,4.237924020083418,4.237924020083418,4.237924020083418,4.237924020083418,4.237924020083418,4.237924020083418,4.237924020083418,4.237924020083418,4.242640687119286,4.242640687119286,4.242640687119286,4.242640687119286,4.242640687119286,4.242640687119286,4.242640687119286,4.242640687119286,4.242640687119286,4.242640687119286,4.242640687119286,4.242640687119286,4.243819034784589,4.243819034784589,4.243819034784589,4.243819034784589,4.243819034784589,4.243819034784589,4.243819034784589,4.243819034784589,4.244997055358225,4.244997055358225,4.244997055358225,4.244997055358225,4.244997055358225,4.244997055358225,4.244997055358225,4.244997055358225,4.244997055358225,4.244997055358225,4.244997055358225,4.244997055358225,4.244997055358225,4.244997055358225,4.244997055358225,4.244997055358225,4.2485291572496005,4.2485291572496005,4.2485291572496005,4.2485291572496005,4.2485291572496005,4.2485291572496005,4.2485291572496005,4.2485291572496005,4.25205832509386,4.25205832509386,4.25205832509386,4.25205832509386,4.25205832509386,4.25205832509386,4.25205832509386,4.25205832509386,4.25440947723653,4.25440947723653,4.25440947723653,4.25440947723653,4.25440947723653,4.25440947723653,4.25440947723653,4.25440947723653,4.25440947723653,4.25440947723653,4.25440947723653,4.25440947723653,4.25440947723653,4.25440947723653,4.25440947723653,4.25440947723653,4.257933771208754,4.257933771208754,4.257933771208754,4.257933771208754,4.257933771208754,4.257933771208754,4.257933771208754,4.257933771208754,4.263801121065568,4.263801121065568,4.263801121065568,4.263801121065568,4.263801121065568,4.263801121065568,4.263801121065568,4.263801121065568,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.272001872658765,4.27551166528639,4.27551166528639,4.27551166528639,4.27551166528639,4.27551166528639,4.27551166528639,4.27551166528639,4.27551166528639,4.280186911806539,4.280186911806539,4.280186911806539,4.280186911806539,4.280186911806539,4.280186911806539,4.280186911806539,4.280186911806539,4.294182110716778,4.294182110716778,4.294182110716778,4.294182110716778,4.294182110716778,4.294182110716778,4.294182110716778,4.294182110716778,4.295346318982906,4.295346318982906,4.295346318982906,4.295346318982906,4.295346318982906,4.295346318982906,4.295346318982906,4.295346318982906,4.295346318982906,4.295346318982906,4.295346318982906,4.295346318982906,4.295346318982906,4.295346318982906,4.295346318982906,4.295346318982906,4.3,4.3,4.3,4.3,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.301162633521314,4.304648650006177,4.304648650006177,4.304648650006177,4.304648650006177,4.304648650006177,4.304648650006177,4.304648650006177,4.304648650006177,4.304648650006177,4.304648650006177,4.304648650006177,4.304648650006177,4.304648650006177,4.304648650006177,4.304648650006177,4.304648650006177,4.308131845707603,4.308131845707603,4.308131845707603,4.308131845707603,4.308131845707603,4.308131845707603,4.308131845707603,4.308131845707603,4.310452412450462,4.310452412450462,4.310452412450462,4.310452412450462,4.310452412450462,4.310452412450462,4.310452412450462,4.310452412450462,4.313930922024599,4.313930922024599,4.313930922024599,4.313930922024599,4.313930922024599,4.313930922024599,4.313930922024599,4.313930922024599,4.317406628984581,4.317406628984581,4.317406628984581,4.317406628984581,4.317406628984581,4.317406628984581,4.317406628984581,4.317406628984581,4.318564576337837,4.318564576337837,4.318564576337837,4.318564576337837,4.318564576337837,4.318564576337837,4.318564576337837,4.318564576337837,4.318564576337837,4.318564576337837,4.318564576337837,4.318564576337837,4.318564576337837,4.318564576337837,4.318564576337837,4.318564576337837,4.3266615305567875,4.3266615305567875,4.3266615305567875,4.3266615305567875,4.3266615305567875,4.3266615305567875,4.3266615305567875,4.3266615305567875,4.327817001676481,4.327817001676481,4.327817001676481,4.327817001676481,4.327817001676481,4.327817001676481,4.327817001676481,4.327817001676481,4.328972164382672,4.328972164382672,4.328972164382672,4.328972164382672,4.328972164382672,4.328972164382672,4.328972164382672,4.328972164382672,4.332435804486894,4.332435804486894,4.332435804486894,4.332435804486894,4.332435804486894,4.332435804486894,4.332435804486894,4.332435804486894,4.338202392696773,4.338202392696773,4.338202392696773,4.338202392696773,4.338202392696773,4.338202392696773,4.338202392696773,4.338202392696773,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.341658669218482,4.346262762420146,4.346262762420146,4.346262762420146,4.346262762420146,4.346262762420146,4.346262762420146,4.346262762420146,4.346262762420146,4.356604182158393,4.356604182158393,4.356604182158393,4.356604182158393,4.356604182158393,4.356604182158393,4.356604182158393,4.356604182158393,4.356604182158393,4.356604182158393,4.356604182158393,4.356604182158393,4.356604182158393,4.356604182158393,4.356604182158393,4.356604182158393,4.360045871318328,4.360045871318328,4.360045871318328,4.360045871318328,4.360045871318328,4.360045871318328,4.360045871318328,4.360045871318328,4.365775990588615,4.365775990588615,4.365775990588615,4.365775990588615,4.365775990588615,4.365775990588615,4.365775990588615,4.365775990588615,4.368065933568311,4.368065933568311,4.368065933568311,4.368065933568311,4.368065933568311,4.368065933568311,4.368065933568311,4.368065933568311,4.373785545725808,4.373785545725808,4.373785545725808,4.373785545725808,4.373785545725808,4.373785545725808,4.373785545725808,4.373785545725808,4.382921400162226,4.382921400162226,4.382921400162226,4.382921400162226,4.382921400162226,4.382921400162226,4.382921400162226,4.382921400162226,4.382921400162226,4.382921400162226,4.382921400162226,4.382921400162226,4.382921400162226,4.382921400162226,4.382921400162226,4.382921400162226,4.384062043356595,4.384062043356595,4.384062043356595,4.384062043356595,4.386342439892262,4.386342439892262,4.386342439892262,4.386342439892262,4.386342439892262,4.386342439892262,4.386342439892262,4.386342439892262,4.386342439892262,4.386342439892262,4.386342439892262,4.386342439892262,4.386342439892262,4.386342439892262,4.386342439892262,4.386342439892262,4.39089968002003,4.39089968002003,4.39089968002003,4.39089968002003,4.39089968002003,4.39089968002003,4.39089968002003,4.39089968002003,4.39317652729776,4.39317652729776,4.39317652729776,4.39317652729776,4.39317652729776,4.39317652729776,4.39317652729776,4.39317652729776,4.39317652729776,4.39317652729776,4.39317652729776,4.39317652729776,4.39317652729776,4.39317652729776,4.39317652729776,4.39317652729776,4.396589587396122,4.396589587396122,4.396589587396122,4.396589587396122,4.396589587396122,4.396589587396122,4.396589587396122,4.396589587396122,4.4,4.4,4.4,4.4,4.401136216933078,4.401136216933078,4.401136216933078,4.401136216933078,4.401136216933078,4.401136216933078,4.401136216933078,4.401136216933078,4.401136216933078,4.401136216933078,4.401136216933078,4.401136216933078,4.401136216933078,4.401136216933078,4.401136216933078,4.401136216933078,4.404543109109048,4.404543109109048,4.404543109109048,4.404543109109048,4.404543109109048,4.404543109109048,4.404543109109048,4.404543109109048,4.404543109109048,4.404543109109048,4.404543109109048,4.404543109109048,4.404543109109048,4.404543109109048,4.404543109109048,4.404543109109048,4.410215414239989,4.410215414239989,4.410215414239989,4.410215414239989,4.410215414239989,4.410215414239989,4.410215414239989,4.410215414239989,4.410215414239989,4.410215414239989,4.410215414239989,4.410215414239989,4.410215414239989,4.410215414239989,4.410215414239989,4.410215414239989,4.4147480109288235,4.4147480109288235,4.4147480109288235,4.4147480109288235,4.4147480109288235,4.4147480109288235,4.4147480109288235,4.4147480109288235,4.418144406874904,4.418144406874904,4.418144406874904,4.418144406874904,4.418144406874904,4.418144406874904,4.418144406874904,4.418144406874904,4.420407221060069,4.420407221060069,4.420407221060069,4.420407221060069,4.420407221060069,4.420407221060069,4.420407221060069,4.420407221060069,4.427188724235731,4.427188724235731,4.427188724235731,4.427188724235731,4.427188724235731,4.427188724235731,4.427188724235731,4.427188724235731,4.428317965096906,4.428317965096906,4.428317965096906,4.428317965096906,4.428317965096906,4.428317965096906,4.428317965096906,4.428317965096906,4.428317965096906,4.428317965096906,4.428317965096906,4.428317965096906,4.428317965096906,4.428317965096906,4.428317965096906,4.428317965096906,4.4294469180700204,4.4294469180700204,4.4294469180700204,4.4294469180700204,4.4294469180700204,4.4294469180700204,4.4294469180700204,4.4294469180700204,4.43846820423443,4.43846820423443,4.43846820423443,4.43846820423443,4.43846820423443,4.43846820423443,4.43846820423443,4.43846820423443,4.43846820423443,4.43846820423443,4.43846820423443,4.43846820423443,4.43846820423443,4.43846820423443,4.43846820423443,4.43846820423443,4.440720662234904,4.440720662234904,4.440720662234904,4.440720662234904,4.440720662234904,4.440720662234904,4.440720662234904,4.440720662234904,4.440720662234904,4.440720662234904,4.440720662234904,4.440720662234904,4.440720662234904,4.440720662234904,4.440720662234904,4.440720662234904,4.4418464629025625,4.4418464629025625,4.4418464629025625,4.4418464629025625,4.4418464629025625,4.4418464629025625,4.4418464629025625,4.4418464629025625,4.455333881989093,4.455333881989093,4.455333881989093,4.455333881989093,4.455333881989093,4.455333881989093,4.455333881989093,4.455333881989093,4.455333881989093,4.455333881989093,4.455333881989093,4.455333881989093,4.455333881989093,4.455333881989093,4.455333881989093,4.455333881989093,4.459820624195552,4.459820624195552,4.459820624195552,4.459820624195552,4.459820624195552,4.459820624195552,4.459820624195552,4.459820624195552,4.459820624195552,4.459820624195552,4.459820624195552,4.459820624195552,4.459820624195552,4.459820624195552,4.459820624195552,4.459820624195552,4.464302857109943,4.464302857109943,4.464302857109943,4.464302857109943,4.464302857109943,4.464302857109943,4.464302857109943,4.464302857109943,4.465422712353222,4.465422712353222,4.465422712353222,4.465422712353222,4.465422712353222,4.465422712353222,4.465422712353222,4.465422712353222,4.468780594300866,4.468780594300866,4.468780594300866,4.468780594300866,4.468780594300866,4.468780594300866,4.468780594300866,4.468780594300866,4.47213595499958,4.47213595499958,4.47213595499958,4.47213595499958,4.47213595499958,4.47213595499958,4.47213595499958,4.47213595499958,4.47213595499958,4.47213595499958,4.47213595499958,4.47213595499958,4.47213595499958,4.47213595499958,4.47213595499958,4.47213595499958,4.4777226354476225,4.4777226354476225,4.4777226354476225,4.4777226354476225,4.4777226354476225,4.4777226354476225,4.4777226354476225,4.4777226354476225,4.4777226354476225,4.4777226354476225,4.4777226354476225,4.4777226354476225,4.4777226354476225,4.4777226354476225,4.4777226354476225,4.4777226354476225,4.4821869662029945,4.4821869662029945,4.4821869662029945,4.4821869662029945,4.4821869662029945,4.4821869662029945,4.4821869662029945,4.4821869662029945,4.491102314577124,4.491102314577124,4.491102314577124,4.491102314577124,4.491102314577124,4.491102314577124,4.491102314577124,4.491102314577124,4.49221548904324,4.49221548904324,4.49221548904324,4.49221548904324,4.49221548904324,4.49221548904324,4.49221548904324,4.49221548904324,4.494441010848846,4.494441010848846,4.494441010848846,4.494441010848846,4.494441010848846,4.494441010848846,4.494441010848846,4.494441010848846,4.494441010848846,4.494441010848846,4.494441010848846,4.494441010848846,4.494441010848846,4.494441010848846,4.494441010848846,4.494441010848846,4.5,4.5,4.5,4.5,4.5,4.5,4.5,4.5,4.5,4.5,4.5,4.5,4.50111097397076,4.50111097397076,4.50111097397076,4.50111097397076,4.50111097397076,4.50111097397076,4.50111097397076,4.50111097397076,4.504442251822083,4.504442251822083,4.504442251822083,4.504442251822083,4.504442251822083,4.504442251822083,4.504442251822083,4.504442251822083,4.509988913511872,4.509988913511872,4.509988913511872,4.509988913511872,4.509988913511872,4.509988913511872,4.509988913511872,4.509988913511872,4.512205669071391,4.512205669071391,4.512205669071391,4.512205669071391,4.512205669071391,4.512205669071391,4.512205669071391,4.512205669071391,4.517742799230607,4.517742799230607,4.517742799230607,4.517742799230607,4.517742799230607,4.517742799230607,4.517742799230607,4.517742799230607,4.517742799230607,4.517742799230607,4.517742799230607,4.517742799230607,4.517742799230607,4.517742799230607,4.517742799230607,4.517742799230607,4.518849411078001,4.518849411078001,4.518849411078001,4.518849411078001,4.518849411078001,4.518849411078001,4.518849411078001,4.518849411078001,4.522167621838006,4.522167621838006,4.522167621838006,4.522167621838006,4.522167621838006,4.522167621838006,4.522167621838006,4.522167621838006,4.522167621838006,4.522167621838006,4.522167621838006,4.522167621838006,4.522167621838006,4.522167621838006,4.522167621838006,4.522167621838006,4.525483399593905,4.525483399593905,4.525483399593905,4.525483399593905,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.527692569068709,4.531004303683677,4.531004303683677,4.531004303683677,4.531004303683677,4.531004303683677,4.531004303683677,4.531004303683677,4.531004303683677,4.534313619501854,4.534313619501854,4.534313619501854,4.534313619501854,4.534313619501854,4.534313619501854,4.534313619501854,4.534313619501854,4.535416188179427,4.535416188179427,4.535416188179427,4.535416188179427,4.535416188179427,4.535416188179427,4.535416188179427,4.535416188179427,4.539823785126467,4.539823785126467,4.539823785126467,4.539823785126467,4.539823785126467,4.539823785126467,4.539823785126467,4.539823785126467,4.545327270945405,4.545327270945405,4.545327270945405,4.545327270945405,4.545327270945405,4.545327270945405,4.545327270945405,4.545327270945405,4.548626166217664,4.548626166217664,4.548626166217664,4.548626166217664,4.548626166217664,4.548626166217664,4.548626166217664,4.548626166217664,4.5541190146942805,4.5541190146942805,4.5541190146942805,4.5541190146942805,4.5541190146942805,4.5541190146942805,4.5541190146942805,4.5541190146942805,4.5541190146942805,4.5541190146942805,4.5541190146942805,4.5541190146942805,4.5541190146942805,4.5541190146942805,4.5541190146942805,4.5541190146942805,4.560701700396552,4.560701700396552,4.560701700396552,4.560701700396552,4.560701700396552,4.560701700396552,4.560701700396552,4.560701700396552,4.560701700396552,4.560701700396552,4.560701700396552,4.560701700396552,4.560701700396552,4.560701700396552,4.560701700396552,4.560701700396552,4.561797891182818,4.561797891182818,4.561797891182818,4.561797891182818,4.561797891182818,4.561797891182818,4.561797891182818,4.561797891182818,4.565084884205331,4.565084884205331,4.565084884205331,4.565084884205331,4.565084884205331,4.565084884205331,4.565084884205331,4.565084884205331,4.5694638635183455,4.5694638635183455,4.5694638635183455,4.5694638635183455,4.5694638635183455,4.5694638635183455,4.5694638635183455,4.5694638635183455,4.570557952810576,4.570557952810576,4.570557952810576,4.570557952810576,4.570557952810576,4.570557952810576,4.570557952810576,4.570557952810576,4.5793012567421245,4.5793012567421245,4.5793012567421245,4.5793012567421245,4.5793012567421245,4.5793012567421245,4.5793012567421245,4.5793012567421245,4.580392996239515,4.580392996239515,4.580392996239515,4.580392996239515,4.580392996239515,4.580392996239515,4.580392996239515,4.580392996239515,4.588027898781784,4.588027898781784,4.588027898781784,4.588027898781784,4.588027898781784,4.588027898781784,4.588027898781784,4.588027898781784,4.588027898781784,4.588027898781784,4.588027898781784,4.588027898781784,4.588027898781784,4.588027898781784,4.588027898781784,4.588027898781784,4.589117562233507,4.589117562233507,4.589117562233507,4.589117562233507,4.589117562233507,4.589117562233507,4.589117562233507,4.589117562233507,4.596737973824482,4.596737973824482,4.596737973824482,4.596737973824482,4.596737973824482,4.596737973824482,4.596737973824482,4.596737973824482,4.6000000000000005,4.6000000000000005,4.6000000000000005,4.6000000000000005,4.601086828130937,4.601086828130937,4.601086828130937,4.601086828130937,4.601086828130937,4.601086828130937,4.601086828130937,4.601086828130937,4.601086828130937,4.601086828130937,4.601086828130937,4.601086828130937,4.601086828130937,4.601086828130937,4.601086828130937,4.601086828130937,4.604345773288535,4.604345773288535,4.604345773288535,4.604345773288535,4.604345773288535,4.604345773288535,4.604345773288535,4.604345773288535,4.604345773288535,4.604345773288535,4.604345773288535,4.604345773288535,4.604345773288535,4.604345773288535,4.604345773288535,4.604345773288535,4.606517122512408,4.606517122512408,4.606517122512408,4.606517122512408,4.606517122512408,4.606517122512408,4.606517122512408,4.606517122512408,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.609772228646444,4.614108798023731,4.614108798023731,4.614108798023731,4.614108798023731,4.614108798023731,4.614108798023731,4.614108798023731,4.614108798023731,4.6173585522460785,4.6173585522460785,4.6173585522460785,4.6173585522460785,4.6173585522460785,4.6173585522460785,4.6173585522460785,4.6173585522460785,4.6173585522460785,4.6173585522460785,4.6173585522460785,4.6173585522460785,4.6173585522460785,4.6173585522460785,4.6173585522460785,4.6173585522460785,4.622769732530489,4.622769732530489,4.622769732530489,4.622769732530489,4.622769732530489,4.622769732530489,4.622769732530489,4.622769732530489,4.623851208678757,4.623851208678757,4.623851208678757,4.623851208678757,4.623851208678757,4.623851208678757,4.623851208678757,4.623851208678757,4.62709412050371,4.62709412050371,4.62709412050371,4.62709412050371,4.62709412050371,4.62709412050371,4.62709412050371,4.62709412050371,4.632493928760188,4.632493928760188,4.632493928760188,4.632493928760188,4.632493928760188,4.632493928760188,4.632493928760188,4.632493928760188,4.632493928760188,4.632493928760188,4.632493928760188,4.632493928760188,4.632493928760188,4.632493928760188,4.632493928760188,4.632493928760188,4.638965401897281,4.638965401897281,4.638965401897281,4.638965401897281,4.638965401897281,4.638965401897281,4.638965401897281,4.638965401897281,4.640043103248073,4.640043103248073,4.640043103248073,4.640043103248073,4.640043103248073,4.640043103248073,4.640043103248073,4.640043103248073,4.648655719667784,4.648655719667784,4.648655719667784,4.648655719667784,4.648655719667784,4.648655719667784,4.648655719667784,4.648655719667784,4.651881339845203,4.651881339845203,4.651881339845203,4.651881339845203,4.651881339845203,4.651881339845203,4.651881339845203,4.651881339845203,4.652956049652737,4.652956049652737,4.652956049652737,4.652956049652737,4.652956049652737,4.652956049652737,4.652956049652737,4.652956049652737,4.652956049652737,4.652956049652737,4.652956049652737,4.652956049652737,4.652956049652737,4.652956049652737,4.652956049652737,4.652956049652737,4.657252408878008,4.657252408878008,4.657252408878008,4.657252408878008,4.657252408878008,4.657252408878008,4.657252408878008,4.657252408878008,4.661544808322666,4.661544808322666,4.661544808322666,4.661544808322666,4.661544808322666,4.661544808322666,4.661544808322666,4.661544808322666,4.661544808322666,4.661544808322666,4.661544808322666,4.661544808322666,4.661544808322666,4.661544808322666,4.661544808322666,4.661544808322666,4.664761515876241,4.664761515876241,4.664761515876241,4.664761515876241,4.664761515876241,4.664761515876241,4.664761515876241,4.664761515876241,4.666904755831214,4.666904755831214,4.666904755831214,4.666904755831214,4.669047011971501,4.669047011971501,4.669047011971501,4.669047011971501,4.669047011971501,4.669047011971501,4.669047011971501,4.669047011971501,4.669047011971501,4.669047011971501,4.669047011971501,4.669047011971501,4.669047011971501,4.669047011971501,4.669047011971501,4.669047011971501,4.675467891024384,4.675467891024384,4.675467891024384,4.675467891024384,4.675467891024384,4.675467891024384,4.675467891024384,4.675467891024384,4.681879964287851,4.681879964287851,4.681879964287851,4.681879964287851,4.681879964287851,4.681879964287851,4.681879964287851,4.681879964287851,4.684015371452148,4.684015371452148,4.684015371452148,4.684015371452148,4.684015371452148,4.684015371452148,4.684015371452148,4.684015371452148,4.686149805543993,4.686149805543993,4.686149805543993,4.686149805543993,4.686149805543993,4.686149805543993,4.686149805543993,4.686149805543993,4.687216658103186,4.687216658103186,4.687216658103186,4.687216658103186,4.687216658103186,4.687216658103186,4.687216658103186,4.687216658103186,4.687216658103186,4.687216658103186,4.687216658103186,4.687216658103186,4.687216658103186,4.687216658103186,4.687216658103186,4.687216658103186,4.695742752749559,4.695742752749559,4.695742752749559,4.695742752749559,4.695742752749559,4.695742752749559,4.695742752749559,4.695742752749559,4.7,4.7,4.7,4.7,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.701063709417264,4.70425339453563,4.70425339453563,4.70425339453563,4.70425339453563,4.70425339453563,4.70425339453563,4.70425339453563,4.70425339453563,4.707440918375928,4.707440918375928,4.707440918375928,4.707440918375928,4.707440918375928,4.707440918375928,4.707440918375928,4.707440918375928,4.709564735726647,4.709564735726647,4.709564735726647,4.709564735726647,4.709564735726647,4.709564735726647,4.709564735726647,4.709564735726647,4.712748667179272,4.712748667179272,4.712748667179272,4.712748667179272,4.712748667179272,4.712748667179272,4.712748667179272,4.712748667179272,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.7169905660283025,4.720169488482379,4.720169488482379,4.720169488482379,4.720169488482379,4.720169488482379,4.720169488482379,4.720169488482379,4.720169488482379,4.72652091923859,4.72652091923859,4.72652091923859,4.72652091923859,4.72652091923859,4.72652091923859,4.72652091923859,4.72652091923859,4.729693436154188,4.729693436154188,4.729693436154188,4.729693436154188,4.729693436154188,4.729693436154188,4.729693436154188,4.729693436154188,4.738143096192854,4.738143096192854,4.738143096192854,4.738143096192854,4.738143096192854,4.738143096192854,4.738143096192854,4.738143096192854,4.738143096192854,4.738143096192854,4.738143096192854,4.738143096192854,4.738143096192854,4.738143096192854,4.738143096192854,4.738143096192854,4.741307836451879,4.741307836451879,4.741307836451879,4.741307836451879,4.741307836451879,4.741307836451879,4.741307836451879,4.741307836451879,4.742362280551751,4.742362280551751,4.742362280551751,4.742362280551751,4.742362280551751,4.742362280551751,4.742362280551751,4.742362280551751,4.742362280551751,4.742362280551751,4.742362280551751,4.742362280551751,4.742362280551751,4.742362280551751,4.742362280551751,4.742362280551751,4.743416490252569,4.743416490252569,4.743416490252569,4.743416490252569,4.743416490252569,4.743416490252569,4.743416490252569,4.743416490252569,4.743416490252569,4.743416490252569,4.743416490252569,4.743416490252569,4.743416490252569,4.743416490252569,4.743416490252569,4.743416490252569,4.750789408087882,4.750789408087882,4.750789408087882,4.750789408087882,4.750789408087882,4.750789408087882,4.750789408087882,4.750789408087882,4.750789408087882,4.750789408087882,4.750789408087882,4.750789408087882,4.750789408087882,4.750789408087882,4.750789408087882,4.750789408087882,4.751841748206689,4.751841748206689,4.751841748206689,4.751841748206689,4.751841748206689,4.751841748206689,4.751841748206689,4.751841748206689,4.753945729601885,4.753945729601885,4.753945729601885,4.753945729601885,4.753945729601885,4.753945729601885,4.753945729601885,4.753945729601885,4.753945729601885,4.753945729601885,4.753945729601885,4.753945729601885,4.753945729601885,4.753945729601885,4.753945729601885,4.753945729601885,4.763402145525822,4.763402145525822,4.763402145525822,4.763402145525822,4.763402145525822,4.763402145525822,4.763402145525822,4.763402145525822,4.767598976424087,4.767598976424087,4.767598976424087,4.767598976424087,4.767598976424087,4.767598976424087,4.767598976424087,4.767598976424087,4.770744176750625,4.770744176750625,4.770744176750625,4.770744176750625,4.770744176750625,4.770744176750625,4.770744176750625,4.770744176750625,4.775981574503821,4.775981574503821,4.775981574503821,4.775981574503821,4.775981574503821,4.775981574503821,4.775981574503821,4.775981574503821,4.780167361086848,4.780167361086848,4.780167361086848,4.780167361086848,4.780167361086848,4.780167361086848,4.780167361086848,4.780167361086848,4.780167361086848,4.780167361086848,4.780167361086848,4.780167361086848,4.780167361086848,4.780167361086848,4.780167361086848,4.780167361086848,4.78539444560216,4.78539444560216,4.78539444560216,4.78539444560216,4.78539444560216,4.78539444560216,4.78539444560216,4.78539444560216,4.78539444560216,4.78539444560216,4.78539444560216,4.78539444560216,4.78539444560216,4.78539444560216,4.78539444560216,4.78539444560216,4.788527957525152,4.788527957525152,4.788527957525152,4.788527957525152,4.788527957525152,4.788527957525152,4.788527957525152,4.788527957525152,4.792702786528704,4.792702786528704,4.792702786528704,4.792702786528704,4.792702786528704,4.792702786528704,4.792702786528704,4.792702786528704,4.800000000000001,4.800000000000001,4.800000000000001,4.800000000000001,4.801041553663122,4.801041553663122,4.801041553663122,4.801041553663122,4.801041553663122,4.801041553663122,4.801041553663122,4.801041553663122,4.801041553663122,4.801041553663122,4.801041553663122,4.801041553663122,4.801041553663122,4.801041553663122,4.801041553663122,4.801041553663122,4.8020828814171885,4.8020828814171885,4.8020828814171885,4.8020828814171885,4.8020828814171885,4.8020828814171885,4.8020828814171885,4.8020828814171885,4.804164859785725,4.804164859785725,4.804164859785725,4.804164859785725,4.804164859785725,4.804164859785725,4.804164859785725,4.804164859785725,4.805205510693586,4.805205510693586,4.805205510693586,4.805205510693586,4.805205510693586,4.805205510693586,4.805205510693586,4.805205510693586,4.8083261120685235,4.8083261120685235,4.8083261120685235,4.8083261120685235,4.8083261120685235,4.8083261120685235,4.8083261120685235,4.8083261120685235,4.8083261120685235,4.8083261120685235,4.8083261120685235,4.8083261120685235,4.80936586256442,4.80936586256442,4.80936586256442,4.80936586256442,4.80936586256442,4.80936586256442,4.80936586256442,4.80936586256442,4.810405388322278,4.810405388322278,4.810405388322278,4.810405388322278,4.810405388322278,4.810405388322278,4.810405388322278,4.810405388322278,4.810405388322278,4.810405388322278,4.810405388322278,4.810405388322278,4.810405388322278,4.810405388322278,4.810405388322278,4.810405388322278,4.8166378315169185,4.8166378315169185,4.8166378315169185,4.8166378315169185,4.8166378315169185,4.8166378315169185,4.8166378315169185,4.8166378315169185,4.8166378315169185,4.8166378315169185,4.8166378315169185,4.8166378315169185,4.8166378315169185,4.8166378315169185,4.8166378315169185,4.8166378315169185,4.825971404805462,4.825971404805462,4.825971404805462,4.825971404805462,4.825971404805462,4.825971404805462,4.825971404805462,4.825971404805462,4.825971404805462,4.825971404805462,4.825971404805462,4.825971404805462,4.825971404805462,4.825971404805462,4.825971404805462,4.825971404805462,4.8270073544588685,4.8270073544588685,4.8270073544588685,4.8270073544588685,4.8270073544588685,4.8270073544588685,4.8270073544588685,4.8270073544588685,4.8270073544588685,4.8270073544588685,4.8270073544588685,4.8270073544588685,4.8270073544588685,4.8270073544588685,4.8270073544588685,4.8270073544588685,4.830113870293329,4.830113870293329,4.830113870293329,4.830113870293329,4.830113870293329,4.830113870293329,4.830113870293329,4.830113870293329,4.833218389437829,4.833218389437829,4.833218389437829,4.833218389437829,4.833218389437829,4.833218389437829,4.833218389437829,4.833218389437829,4.8373546489791295,4.8373546489791295,4.8373546489791295,4.8373546489791295,4.8373546489791295,4.8373546489791295,4.8373546489791295,4.8373546489791295,4.8373546489791295,4.8373546489791295,4.8373546489791295,4.8373546489791295,4.8373546489791295,4.8373546489791295,4.8373546489791295,4.8373546489791295,4.838388161361178,4.838388161361178,4.838388161361178,4.838388161361178,4.838388161361178,4.838388161361178,4.838388161361178,4.838388161361178,4.841487374764082,4.841487374764082,4.841487374764082,4.841487374764082,4.841487374764082,4.841487374764082,4.841487374764082,4.841487374764082,4.846648326421054,4.846648326421054,4.846648326421054,4.846648326421054,4.846648326421054,4.846648326421054,4.846648326421054,4.846648326421054,4.850773134253962,4.850773134253962,4.850773134253962,4.850773134253962,4.850773134253962,4.850773134253962,4.850773134253962,4.850773134253962,4.850773134253962,4.850773134253962,4.850773134253962,4.850773134253962,4.850773134253962,4.850773134253962,4.850773134253962,4.850773134253962,4.854894437575343,4.854894437575343,4.854894437575343,4.854894437575343,4.854894437575343,4.854894437575343,4.854894437575343,4.854894437575343,4.860041152089147,4.860041152089147,4.860041152089147,4.860041152089147,4.860041152089147,4.860041152089147,4.860041152089147,4.860041152089147,4.866210024238576,4.866210024238576,4.866210024238576,4.866210024238576,4.866210024238576,4.866210024238576,4.866210024238576,4.866210024238576,4.870318264754369,4.870318264754369,4.870318264754369,4.870318264754369,4.870318264754369,4.870318264754369,4.870318264754369,4.870318264754369,4.8754486972995625,4.8754486972995625,4.8754486972995625,4.8754486972995625,4.8754486972995625,4.8754486972995625,4.8754486972995625,4.8754486972995625,4.876474136094644,4.876474136094644,4.876474136094644,4.876474136094644,4.876474136094644,4.876474136094644,4.876474136094644,4.876474136094644,4.876474136094644,4.876474136094644,4.876474136094644,4.876474136094644,4.876474136094644,4.876474136094644,4.876474136094644,4.876474136094644,4.879549159502341,4.879549159502341,4.879549159502341,4.879549159502341,4.879549159502341,4.879549159502341,4.879549159502341,4.879549159502341,4.882622246293481,4.882622246293481,4.882622246293481,4.882622246293481,4.882622246293481,4.882622246293481,4.882622246293481,4.882622246293481,4.883646178829912,4.883646178829912,4.883646178829912,4.883646178829912,4.883646178829912,4.883646178829912,4.883646178829912,4.883646178829912,4.883646178829912,4.883646178829912,4.883646178829912,4.883646178829912,4.883646178829912,4.883646178829912,4.883646178829912,4.883646178829912,4.884669896727925,4.884669896727925,4.884669896727925,4.884669896727925,4.884669896727925,4.884669896727925,4.884669896727925,4.884669896727925,4.887739763939975,4.887739763939975,4.887739763939975,4.887739763939975,4.887739763939975,4.887739763939975,4.887739763939975,4.887739763939975,4.891829923454004,4.891829923454004,4.891829923454004,4.891829923454004,4.891829923454004,4.891829923454004,4.891829923454004,4.891829923454004,4.9,4.9,4.9,4.9,4.901020301937138,4.901020301937138,4.901020301937138,4.901020301937138,4.901020301937138,4.901020301937138,4.901020301937138,4.901020301937138,4.903060268852506,4.903060268852506,4.903060268852506,4.903060268852506,4.903060268852506,4.903060268852506,4.903060268852506,4.903060268852506,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.904079934095692,4.909175083453431,4.909175083453431,4.909175083453431,4.909175083453431,4.909175083453431,4.909175083453431,4.909175083453431,4.909175083453431,4.909175083453431,4.909175083453431,4.909175083453431,4.909175083453431,4.909175083453431,4.909175083453431,4.909175083453431,4.909175083453431,4.9162994213127424,4.9162994213127424,4.9162994213127424,4.9162994213127424,4.9162994213127424,4.9162994213127424,4.9162994213127424,4.9162994213127424,4.919349550499538,4.919349550499538,4.919349550499538,4.919349550499538,4.919349550499538,4.919349550499538,4.919349550499538,4.919349550499538,4.920365840057018,4.920365840057018,4.920365840057018,4.920365840057018,4.920365840057018,4.920365840057018,4.920365840057018,4.920365840057018,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.924428900898053,4.925444142409901,4.925444142409901,4.925444142409901,4.925444142409901,4.925444142409901,4.925444142409901,4.925444142409901,4.925444142409901,4.933558553417605,4.933558553417605,4.933558553417605,4.933558553417605,4.933558553417605,4.933558553417605,4.933558553417605,4.933558553417605,4.9365980188789935,4.9365980188789935,4.9365980188789935,4.9365980188789935,4.9365980188789935,4.9365980188789935,4.9365980188789935,4.9365980188789935,4.9396356140913875,4.9396356140913875,4.9396356140913875,4.9396356140913875,4.9396356140913875,4.9396356140913875,4.9396356140913875,4.9396356140913875,4.9396356140913875,4.9396356140913875,4.9396356140913875,4.9396356140913875,4.9396356140913875,4.9396356140913875,4.9396356140913875,4.9396356140913875,4.94064773081425,4.94064773081425,4.94064773081425,4.94064773081425,4.94064773081425,4.94064773081425,4.94064773081425,4.94064773081425,4.947726750741193,4.947726750741193,4.947726750741193,4.947726750741193,4.947726750741193,4.947726750741193,4.947726750741193,4.947726750741193,4.949747468305834,4.949747468305834,4.949747468305834,4.949747468305834,4.949747468305834,4.949747468305834,4.949747468305834,4.949747468305834,4.949747468305834,4.949747468305834,4.949747468305834,4.949747468305834,4.951767361255979,4.951767361255979,4.951767361255979,4.951767361255979,4.951767361255979,4.951767361255979,4.951767361255979,4.951767361255979,4.957822102496216,4.957822102496216,4.957822102496216,4.957822102496216,4.957822102496216,4.957822102496216,4.957822102496216,4.957822102496216,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.964876634922565,4.965883607174055,4.965883607174055,4.965883607174055,4.965883607174055,4.965883607174055,4.965883607174055,4.965883607174055,4.965883607174055,4.967896939349689,4.967896939349689,4.967896939349689,4.967896939349689,4.967896939349689,4.967896939349689,4.967896939349689,4.967896939349689,4.972926703662543,4.972926703662543,4.972926703662543,4.972926703662543,4.972926703662543,4.972926703662543,4.972926703662543,4.972926703662543,4.973932046178355,4.973932046178355,4.973932046178355,4.973932046178355,4.973932046178355,4.973932046178355,4.973932046178355,4.973932046178355,4.976946855251722,4.976946855251722,4.976946855251722,4.976946855251722,4.976946855251722,4.976946855251722,4.976946855251722,4.976946855251722,4.98196748283246,4.98196748283246,4.98196748283246,4.98196748283246,4.98196748283246,4.98196748283246,4.98196748283246,4.98196748283246,4.98196748283246,4.98196748283246,4.98196748283246,4.98196748283246,4.98196748283246,4.98196748283246,4.98196748283246,4.98196748283246,4.992995093127972,4.992995093127972,4.992995093127972,4.992995093127972,4.992995093127972,4.992995093127972,4.992995093127972,4.992995093127972,4.99799959983992,4.99799959983992,4.99799959983992,4.99799959983992,4.99799959983992,4.99799959983992,4.99799959983992,4.99799959983992,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0,5.0}; + +const int expansion_pos_l[7845] = {0,0,0,1,-1,1,1,-1,-1,0,0,2,-2,1,1,-1,-1,2,2,-2,-2,2,2,-2,-2,0,0,3,-3,1,1,-1,-1,3,3,-3,-3,2,2,-2,-2,3,3,-3,-3,0,0,4,-4,1,1,-1,-1,4,4,-4,-4,3,3,-3,-3,2,2,-2,-2,4,4,-4,-4,0,0,3,3,-3,-3,4,4,-4,-4,5,-5,1,1,-1,-1,5,5,-5,-5,2,2,-2,-2,5,5,-5,-5,4,4,-4,-4,3,3,-3,-3,5,5,-5,-5,0,0,6,-6,1,1,-1,-1,6,6,-6,-6,2,2,-2,-2,6,6,-6,-6,4,4,-4,-4,5,5,-5,-5,3,3,-3,-3,6,6,-6,-6,0,0,7,-7,1,1,-1,-1,5,5,-5,-5,7,7,-7,-7,4,4,-4,-4,6,6,-6,-6,2,2,-2,-2,7,7,-7,-7,3,3,-3,-3,7,7,-7,-7,5,5,-5,-5,6,6,-6,-6,0,0,8,-8,1,1,-1,-1,4,4,-4,-4,7,7,-7,-7,8,8,-8,-8,2,2,-2,-2,8,8,-8,-8,6,6,-6,-6,3,3,-3,-3,8,8,-8,-8,5,5,-5,-5,7,7,-7,-7,4,4,-4,-4,8,8,-8,-8,0,0,9,-9,1,1,-1,-1,9,9,-9,-9,2,2,-2,-2,6,6,-6,-6,7,7,-7,-7,9,9,-9,-9,5,5,-5,-5,8,8,-8,-8,3,3,-3,-3,9,9,-9,-9,4,4,-4,-4,9,9,-9,-9,7,7,-7,-7,0,0,6,6,-6,-6,8,8,-8,-8,10,-10,1,1,-1,-1,10,10,-10,-10,2,2,-2,-2,10,10,-10,-10,5,5,-5,-5,9,9,-9,-9,3,3,-3,-3,10,10,-10,-10,7,7,-7,-7,8,8,-8,-8,4,4,-4,-4,10,10,-10,-10,6,6,-6,-6,9,9,-9,-9,0,0,11,-11,1,1,-1,-1,11,11,-11,-11,2,2,-2,-2,5,5,-5,-5,10,10,-10,-10,11,11,-11,-11,8,8,-8,-8,3,3,-3,-3,7,7,-7,-7,9,9,-9,-9,11,11,-11,-11,6,6,-6,-6,10,10,-10,-10,4,4,-4,-4,11,11,-11,-11,0,0,12,-12,1,1,-1,-1,8,8,-8,-8,9,9,-9,-9,12,12,-12,-12,5,5,-5,-5,11,11,-11,-11,2,2,-2,-2,12,12,-12,-12,7,7,-7,-7,10,10,-10,-10,3,3,-3,-3,12,12,-12,-12,6,6,-6,-6,11,11,-11,-11,4,4,-4,-4,12,12,-12,-12,9,9,-9,-9,8,8,-8,-8,10,10,-10,-10,0,0,5,5,-5,-5,12,12,-12,-12,13,-13,1,1,-1,-1,7,7,-7,-7,11,11,-11,-11,13,13,-13,-13,2,2,-2,-2,13,13,-13,-13,3,3,-3,-3,13,13,-13,-13,6,6,-6,-6,12,12,-12,-12,9,9,-9,-9,10,10,-10,-10,4,4,-4,-4,8,8,-8,-8,11,11,-11,-11,13,13,-13,-13,7,7,-7,-7,12,12,-12,-12,5,5,-5,-5,13,13,-13,-13,0,0,14,-14,1,1,-1,-1,14,14,-14,-14,2,2,-2,-2,10,10,-10,-10,14,14,-14,-14,9,9,-9,-9,11,11,-11,-11,3,3,-3,-3,6,6,-6,-6,13,13,-13,-13,14,14,-14,-14,8,8,-8,-8,12,12,-12,-12,4,4,-4,-4,14,14,-14,-14,7,7,-7,-7,13,13,-13,-13,5,5,-5,-5,10,10,-10,-10,11,11,-11,-11,14,14,-14,-14,0,0,9,9,-9,-9,12,12,-12,-12,15,-15,1,1,-1,-1,15,15,-15,-15,2,2,-2,-2,15,15,-15,-15,6,6,-6,-6,14,14,-14,-14,8,8,-8,-8,13,13,-13,-13,3,3,-3,-3,15,15,-15,-15,4,4,-4,-4,15,15,-15,-15,11,11,-11,-11,10,10,-10,-10,12,12,-12,-12,7,7,-7,-7,14,14,-14,-14,5,5,-5,-5,9,9,-9,-9,13,13,-13,-13,15,15,-15,-15,0,0,16,-16,1,1,-1,-1,16,16,-16,-16,2,2,-2,-2,8,8,-8,-8,14,14,-14,-14,16,16,-16,-16,6,6,-6,-6,15,15,-15,-15,3,3,-3,-3,11,11,-11,-11,12,12,-12,-12,16,16,-16,-16,10,10,-10,-10,13,13,-13,-13,4,4,-4,-4,16,16,-16,-16,7,7,-7,-7,15,15,-15,-15,9,9,-9,-9,14,14,-14,-14,5,5,-5,-5,16,16,-16,-16,12,12,-12,-12,0,0,8,8,-8,-8,15,15,-15,-15,17,-17,1,1,-1,-1,11,11,-11,-11,13,13,-13,-13,17,17,-17,-17,6,6,-6,-6,16,16,-16,-16,2,2,-2,-2,17,17,-17,-17,10,10,-10,-10,14,14,-14,-14,3,3,-3,-3,17,17,-17,-17,4,4,-4,-4,7,7,-7,-7,16,16,-16,-16,17,17,-17,-17,9,9,-9,-9,15,15,-15,-15,12,12,-12,-12,13,13,-13,-13,5,5,-5,-5,17,17,-17,-17,11,11,-11,-11,14,14,-14,-14,8,8,-8,-8,16,16,-16,-16,0,0,18,-18,1,1,-1,-1,6,6,-6,-6,10,10,-10,-10,15,15,-15,-15,17,17,-17,-17,18,18,-18,-18,2,2,-2,-2,18,18,-18,-18,3,3,-3,-3,18,18,-18,-18,9,9,-9,-9,16,16,-16,-16,7,7,-7,-7,13,13,-13,-13,17,17,-17,-17,4,4,-4,-4,12,12,-12,-12,14,14,-14,-14,18,18,-18,-18,11,11,-11,-11,15,15,-15,-15,5,5,-5,-5,18,18,-18,-18,8,8,-8,-8,17,17,-17,-17,10,10,-10,-10,16,16,-16,-16,6,6,-6,-6,18,18,-18,-18,0,0,19,-19,1,1,-1,-1,19,19,-19,-19,2,2,-2,-2,13,13,-13,-13,14,14,-14,-14,19,19,-19,-19,12,12,-12,-12,15,15,-15,-15,3,3,-3,-3,9,9,-9,-9,17,17,-17,-17,19,19,-19,-19,7,7,-7,-7,18,18,-18,-18,4,4,-4,-4,11,11,-11,-11,16,16,-16,-16,19,19,-19,-19,5,5,-5,-5,19,19,-19,-19,8,8,-8,-8,18,18,-18,-18,10,10,-10,-10,17,17,-17,-17,14,14,-14,-14,13,13,-13,-13,15,15,-15,-15,6,6,-6,-6,19,19,-19,-19,0,0,12,12,-12,-12,16,16,-16,-16,20,-20,1,1,-1,-1,20,20,-20,-20,2,2,-2,-2,20,20,-20,-20,9,9,-9,-9,18,18,-18,-18,3,3,-3,-3,20,20,-20,-20,7,7,-7,-7,11,11,-11,-11,17,17,-17,-17,19,19,-19,-19,4,4,-4,-4,20,20,-20,-20,14,14,-14,-14,15,15,-15,-15,10,10,-10,-10,18,18,-18,-18,5,5,-5,-5,8,8,-8,-8,13,13,-13,-13,16,16,-16,-16,19,19,-19,-19,20,20,-20,-20,12,12,-12,-12,17,17,-17,-17,6,6,-6,-6,20,20,-20,-20,0,0,21,-21,1,1,-1,-1,9,9,-9,-9,19,19,-19,-19,21,21,-21,-21,2,2,-2,-2,11,11,-11,-11,18,18,-18,-18,21,21,-21,-21,7,7,-7,-7,20,20,-20,-20,3,3,-3,-3,15,15,-15,-15,21,21,-21,-21,14,14,-14,-14,16,16,-16,-16,4,4,-4,-4,21,21,-21,-21,13,13,-13,-13,17,17,-17,-17,10,10,-10,-10,19,19,-19,-19,8,8,-8,-8,20,20,-20,-20,5,5,-5,-5,21,21,-21,-21,12,12,-12,-12,18,18,-18,-18,6,6,-6,-6,21,21,-21,-21,9,9,-9,-9,15,15,-15,-15,16,16,-16,-16,20,20,-20,-20,11,11,-11,-11,19,19,-19,-19,0,0,22,-22,1,1,-1,-1,14,14,-14,-14,17,17,-17,-17,22,22,-22,-22,2,2,-2,-2,22,22,-22,-22,7,7,-7,-7,21,21,-21,-21,3,3,-3,-3,13,13,-13,-13,18,18,-18,-18,22,22,-22,-22,4,4,-4,-4,10,10,-10,-10,20,20,-20,-20,22,22,-22,-22,8,8,-8,-8,12,12,-12,-12,19,19,-19,-19,21,21,-21,-21,5,5,-5,-5,22,22,-22,-22,16,16,-16,-16,15,15,-15,-15,17,17,-17,-17,6,6,-6,-6,14,14,-14,-14,18,18,-18,-18,22,22,-22,-22,11,11,-11,-11,20,20,-20,-20,9,9,-9,-9,21,21,-21,-21,0,0,23,-23,1,1,-1,-1,13,13,-13,-13,19,19,-19,-19,23,23,-23,-23,2,2,-2,-2,7,7,-7,-7,22,22,-22,-22,23,23,-23,-23,3,3,-3,-3,23,23,-23,-23,10,10,-10,-10,21,21,-21,-21,12,12,-12,-12,20,20,-20,-20,4,4,-4,-4,16,16,-16,-16,17,17,-17,-17,23,23,-23,-23,8,8,-8,-8,22,22,-22,-22,15,15,-15,-15,18,18,-18,-18,5,5,-5,-5,23,23,-23,-23,14,14,-14,-14,19,19,-19,-19,11,11,-11,-11,21,21,-21,-21,6,6,-6,-6,9,9,-9,-9,22,22,-22,-22,23,23,-23,-23,13,13,-13,-13,20,20,-20,-20,0,0,24,-24,1,1,-1,-1,24,24,-24,-24,7,7,-7,-7,17,17,-17,-17,23,23,-23,-23,2,2,-2,-2,16,16,-16,-16,18,18,-18,-18,24,24,-24,-24,10,10,-10,-10,22,22,-22,-22,3,3,-3,-3,12,12,-12,-12,21,21,-21,-21,24,24,-24,-24,15,15,-15,-15,19,19,-19,-19,4,4,-4,-4,24,24,-24,-24,8,8,-8,-8,23,23,-23,-23,14,14,-14,-14,20,20,-20,-20,5,5,-5,-5,24,24,-24,-24,11,11,-11,-11,22,22,-22,-22,9,9,-9,-9,13,13,-13,-13,21,21,-21,-21,23,23,-23,-23,6,6,-6,-6,24,24,-24,-24,17,17,-17,-17,18,18,-18,-18,16,16,-16,-16,19,19,-19,-19,0,0,7,7,-7,-7,15,15,-15,-15,20,20,-20,-20,24,24,-24,-24,25,-25,1,1,-1,-1,25,25,-25,-25,12,12,-12,-12,22,22,-22,-22,2,2,-2,-2,10,10,-10,-10,23,23,-23,-23,25,25,-25,-25,3,3,-3,-3,25,25,-25,-25,14,14,-14,-14,21,21,-21,-21,8,8,-8,-8,24,24,-24,-24,4,4,-4,-4,25,25,-25,-25,18,18,-18,-18,5,5,-5,-5,11,11,-11,-11,17,17,-17,-17,19,19,-19,-19,23,23,-23,-23,25,25,-25,-25,13,13,-13,-13,22,22,-22,-22,16,16,-16,-16,20,20,-20,-20,9,9,-9,-9,24,24,-24,-24,6,6,-6,-6,25,25,-25,-25,15,15,-15,-15,21,21,-21,-21,12,12,-12,-12,23,23,-23,-23,7,7,-7,-7,25,25,-25,-25,0,0,10,10,-10,-10,24,24,-24,-24,26,-26,1,1,-1,-1,26,26,-26,-26,2,2,-2,-2,14,14,-14,-14,22,22,-22,-22,26,26,-26,-26,3,3,-3,-3,18,18,-18,-18,19,19,-19,-19,26,26,-26,-26,8,8,-8,-8,17,17,-17,-17,20,20,-20,-20,25,25,-25,-25,4,4,-4,-4,26,26,-26,-26,11,11,-11,-11,16,16,-16,-16,21,21,-21,-21,24,24,-24,-24,13,13,-13,-13,23,23,-23,-23,5,5,-5,-5,26,26,-26,-26,9,9,-9,-9,25,25,-25,-25,15,15,-15,-15,22,22,-22,-22,6,6,-6,-6,26,26,-26,-26,12,12,-12,-12,24,24,-24,-24,19,19,-19,-19,18,18,-18,-18,20,20,-20,-20,7,7,-7,-7,10,10,-10,-10,14,14,-14,-14,23,23,-23,-23,25,25,-25,-25,26,26,-26,-26,0,0,27,-27,1,1,-1,-1,17,17,-17,-17,21,21,-21,-21,27,27,-27,-27,2,2,-2,-2,27,27,-27,-27,3,3,-3,-3,27,27,-27,-27,8,8,-8,-8,16,16,-16,-16,22,22,-22,-22,26,26,-26,-26,4,4,-4,-4,13,13,-13,-13,24,24,-24,-24,27,27,-27,-27,11,11,-11,-11,25,25,-25,-25,5,5,-5,-5,15,15,-15,-15,23,23,-23,-23,27,27,-27,-27,9,9,-9,-9,26,26,-26,-26,19,19,-19,-19,20,20,-20,-20,6,6,-6,-6,18,18,-18,-18,21,21,-21,-21,27,27,-27,-27,12,12,-12,-12,25,25,-25,-25,14,14,-14,-14,24,24,-24,-24,17,17,-17,-17,22,22,-22,-22,10,10,-10,-10,26,26,-26,-26,7,7,-7,-7,27,27,-27,-27,0,0,28,-28,1,1,-1,-1,16,16,-16,-16,23,23,-23,-23,28,28,-28,-28,2,2,-2,-2,28,28,-28,-28,3,3,-3,-3,8,8,-8,-8,27,27,-27,-27,28,28,-28,-28,13,13,-13,-13,25,25,-25,-25,11,11,-11,-11,26,26,-26,-26,4,4,-4,-4,20,20,-20,-20,28,28,-28,-28,15,15,-15,-15,24,24,-24,-24,19,19,-19,-19,21,21,-21,-21,18,18,-18,-18,22,22,-22,-22,5,5,-5,-5,28,28,-28,-28,9,9,-9,-9,27,27,-27,-27,17,17,-17,-17,23,23,-23,-23,6,6,-6,-6,12,12,-12,-12,26,26,-26,-26,28,28,-28,-28,14,14,-14,-14,25,25,-25,-25,10,10,-10,-10,27,27,-27,-27,16,16,-16,-16,24,24,-24,-24,7,7,-7,-7,28,28,-28,-28,0,0,20,20,-20,-20,21,21,-21,-21,29,-29,1,1,-1,-1,29,29,-29,-29,2,2,-2,-2,13,13,-13,-13,19,19,-19,-19,22,22,-22,-22,26,26,-26,-26,29,29,-29,-29,8,8,-8,-8,28,28,-28,-28,3,3,-3,-3,11,11,-11,-11,15,15,-15,-15,25,25,-25,-25,27,27,-27,-27,29,29,-29,-29,18,18,-18,-18,23,23,-23,-23,4,4,-4,-4,29,29,-29,-29,9,9,-9,-9,17,17,-17,-17,24,24,-24,-24,28,28,-28,-28,5,5,-5,-5,29,29,-29,-29,14,14,-14,-14,26,26,-26,-26,12,12,-12,-12,27,27,-27,-27,6,6,-6,-6,29,29,-29,-29,16,16,-16,-16,25,25,-25,-25,21,21,-21,-21,10,10,-10,-10,20,20,-20,-20,22,22,-22,-22,28,28,-28,-28,7,7,-7,-7,19,19,-19,-19,23,23,-23,-23,29,29,-29,-29,13,13,-13,-13,27,27,-27,-27,0,0,18,18,-18,-18,24,24,-24,-24,30,-30,1,1,-1,-1,15,15,-15,-15,26,26,-26,-26,30,30,-30,-30,2,2,-2,-2,30,30,-30,-30,8,8,-8,-8,11,11,-11,-11,28,28,-28,-28,29,29,-29,-29,3,3,-3,-3,30,30,-30,-30,17,17,-17,-17,25,25,-25,-25,4,4,-4,-4,30,30,-30,-30,9,9,-9,-9,29,29,-29,-29,5,5,-5,-5,14,14,-14,-14,21,21,-21,-21,22,22,-22,-22,27,27,-27,-27,30,30,-30,-30,12,12,-12,-12,28,28,-28,-28,20,20,-20,-20,23,23,-23,-23,16,16,-16,-16,26,26,-26,-26,6,6,-6,-6,30,30,-30,-30,19,19,-19,-19,24,24,-24,-24,10,10,-10,-10,29,29,-29,-29,7,7,-7,-7,18,18,-18,-18,25,25,-25,-25,30,30,-30,-30,13,13,-13,-13,28,28,-28,-28,15,15,-15,-15,27,27,-27,-27,0,0,31,-31,1,1,-1,-1,11,11,-11,-11,29,29,-29,-29,31,31,-31,-31,8,8,-8,-8,30,30,-30,-30,2,2,-2,-2,17,17,-17,-17,26,26,-26,-26,31,31,-31,-31,22,22,-22,-22,3,3,-3,-3,21,21,-21,-21,23,23,-23,-23,31,31,-31,-31,20,20,-20,-20,24,24,-24,-24,4,4,-4,-4,31,31,-31,-31,14,14,-14,-14,28,28,-28,-28,9,9,-9,-9,30,30,-30,-30,12,12,-12,-12,16,16,-16,-16,27,27,-27,-27,29,29,-29,-29,5,5,-5,-5,19,19,-19,-19,25,25,-25,-25,31,31,-31,-31,6,6,-6,-6,31,31,-31,-31,10,10,-10,-10,18,18,-18,-18,26,26,-26,-26,30,30,-30,-30,15,15,-15,-15,28,28,-28,-28,7,7,-7,-7,13,13,-13,-13,29,29,-29,-29,31,31,-31,-31,22,22,-22,-22,23,23,-23,-23,21,21,-21,-21,24,24,-24,-24,17,17,-17,-17,27,27,-27,-27,11,11,-11,-11,30,30,-30,-30,0,0,32,-32,1,1,-1,-1,8,8,-8,-8,20,20,-20,-20,25,25,-25,-25,31,31,-31,-31,32,32,-32,-32,2,2,-2,-2,32,32,-32,-32,3,3,-3,-3,32,32,-32,-32,14,14,-14,-14,19,19,-19,-19,26,26,-26,-26,29,29,-29,-29,4,4,-4,-4,16,16,-16,-16,28,28,-28,-28,32,32,-32,-32,9,9,-9,-9,31,31,-31,-31,12,12,-12,-12,30,30,-30,-30,5,5,-5,-5,32,32,-32,-32,18,18,-18,-18,27,27,-27,-27,23,23,-23,-23,6,6,-6,-6,22,22,-22,-22,24,24,-24,-24,32,32,-32,-32,10,10,-10,-10,31,31,-31,-31,15,15,-15,-15,21,21,-21,-21,25,25,-25,-25,29,29,-29,-29,13,13,-13,-13,30,30,-30,-30,7,7,-7,-7,17,17,-17,-17,28,28,-28,-28,32,32,-32,-32,20,20,-20,-20,26,26,-26,-26,11,11,-11,-11,31,31,-31,-31,8,8,-8,-8,32,32,-32,-32,0,0,33,-33,1,1,-1,-1,19,19,-19,-19,27,27,-27,-27,33,33,-33,-33,2,2,-2,-2,33,33,-33,-33,14,14,-14,-14,30,30,-30,-30,16,16,-16,-16,29,29,-29,-29,3,3,-3,-3,33,33,-33,-33,4,4,-4,-4,9,9,-9,-9,12,12,-12,-12,23,23,-23,-23,24,24,-24,-24,31,31,-31,-31,32,32,-32,-32,33,33,-33,-33,18,18,-18,-18,28,28,-28,-28,22,22,-22,-22,25,25,-25,-25,5,5,-5,-5,33,33,-33,-33,21,21,-21,-21,26,26,-26,-26,10,10,-10,-10,32,32,-32,-32,6,6,-6,-6,15,15,-15,-15,30,30,-30,-30,33,33,-33,-33,20,20,-20,-20,27,27,-27,-27,13,13,-13,-13,17,17,-17,-17,29,29,-29,-29,31,31,-31,-31,7,7,-7,-7,33,33,-33,-33,11,11,-11,-11,19,19,-19,-19,28,28,-28,-28,32,32,-32,-32,24,24,-24,-24,8,8,-8,-8,33,33,-33,-33,23,23,-23,-23,25,25,-25,-25,0,0,16,16,-16,-16,30,30,-30,-30,34,-34,1,1,-1,-1,14,14,-14,-14,31,31,-31,-31,34,34,-34,-34,2,2,-2,-2,22,22,-22,-22,26,26,-26,-26,34,34,-34,-34,3,3,-3,-3,18,18,-18,-18,29,29,-29,-29,34,34,-34,-34,12,12,-12,-12,32,32,-32,-32,9,9,-9,-9,21,21,-21,-21,27,27,-27,-27,33,33,-33,-33,4,4,-4,-4,34,34,-34,-34,5,5,-5,-5,34,34,-34,-34,20,20,-20,-20,28,28,-28,-28,15,15,-15,-15,31,31,-31,-31,10,10,-10,-10,17,17,-17,-17,30,30,-30,-30,33,33,-33,-33,6,6,-6,-6,34,34,-34,-34,13,13,-13,-13,32,32,-32,-32,24,24,-24,-24,25,25,-25,-25,19,19,-19,-19,29,29,-29,-29,7,7,-7,-7,23,23,-23,-23,26,26,-26,-26,34,34,-34,-34,11,11,-11,-11,33,33,-33,-33,22,22,-22,-22,27,27,-27,-27,16,16,-16,-16,31,31,-31,-31,8,8,-8,-8,14,14,-14,-14,32,32,-32,-32,34,34,-34,-34,18,18,-18,-18,30,30,-30,-30,0,0,21,21,-21,-21,28,28,-28,-28,35,-35,1,1,-1,-1,35,35,-35,-35,2,2,-2,-2,35,35,-35,-35,12,12,-12,-12,33,33,-33,-33,3,3,-3,-3,35,35,-35,-35,9,9,-9,-9,34,34,-34,-34,4,4,-4,-4,20,20,-20,-20,29,29,-29,-29,35,35,-35,-35,15,15,-15,-15,32,32,-32,-32,5,5,-5,-5,17,17,-17,-17,25,25,-25,-25,31,31,-31,-31,35,35,-35,-35,24,24,-24,-24,26,26,-26,-26,10,10,-10,-10,34,34,-34,-34,13,13,-13,-13,23,23,-23,-23,27,27,-27,-27,33,33,-33,-33,6,6,-6,-6,19,19,-19,-19,30,30,-30,-30,35,35,-35,-35,22,22,-22,-22,28,28,-28,-28,7,7,-7,-7,35,35,-35,-35,11,11,-11,-11,34,34,-34,-34,16,16,-16,-16,32,32,-32,-32,21,21,-21,-21,29,29,-29,-29,14,14,-14,-14,18,18,-18,-18,31,31,-31,-31,33,33,-33,-33,8,8,-8,-8,35,35,-35,-35,0,0,36,-36,1,1,-1,-1,36,36,-36,-36,2,2,-2,-2,12,12,-12,-12,20,20,-20,-20,30,30,-30,-30,34,34,-34,-34,36,36,-36,-36,25,25,-25,-25,26,26,-26,-26,3,3,-3,-3,24,24,-24,-24,27,27,-27,-27,36,36,-36,-36,9,9,-9,-9,35,35,-35,-35,4,4,-4,-4,36,36,-36,-36,17,17,-17,-17,23,23,-23,-23,28,28,-28,-28,32,32,-32,-32,15,15,-15,-15,33,33,-33,-33,5,5,-5,-5,36,36,-36,-36,19,19,-19,-19,31,31,-31,-31,10,10,-10,-10,13,13,-13,-13,22,22,-22,-22,29,29,-29,-29,34,34,-34,-34,35,35,-35,-35,6,6,-6,-6,36,36,-36,-36,21,21,-21,-21,30,30,-30,-30,7,7,-7,-7,16,16,-16,-16,33,33,-33,-33,36,36,-36,-36,11,11,-11,-11,35,35,-35,-35,18,18,-18,-18,32,32,-32,-32,14,14,-14,-14,26,26,-26,-26,34,34,-34,-34,25,25,-25,-25,27,27,-27,-27,8,8,-8,-8,24,24,-24,-24,28,28,-28,-28,36,36,-36,-36,20,20,-20,-20,31,31,-31,-31,0,0,12,12,-12,-12,35,35,-35,-35,37,-37,1,1,-1,-1,23,23,-23,-23,29,29,-29,-29,37,37,-37,-37,2,2,-2,-2,37,37,-37,-37,9,9,-9,-9,36,36,-36,-36,3,3,-3,-3,17,17,-17,-17,33,33,-33,-33,37,37,-37,-37,15,15,-15,-15,34,34,-34,-34,22,22,-22,-22,30,30,-30,-30,4,4,-4,-4,19,19,-19,-19,32,32,-32,-32,37,37,-37,-37,5,5,-5,-5,13,13,-13,-13,35,35,-35,-35,37,37,-37,-37,10,10,-10,-10,36,36,-36,-36,21,21,-21,-21,31,31,-31,-31,6,6,-6,-6,26,26,-26,-26,27,27,-27,-27,37,37,-37,-37,25,25,-25,-25,28,28,-28,-28,16,16,-16,-16,34,34,-34,-34,18,18,-18,-18,33,33,-33,-33,11,11,-11,-11,24,24,-24,-24,29,29,-29,-29,36,36,-36,-36,7,7,-7,-7,37,37,-37,-37,14,14,-14,-14,35,35,-35,-35,20,20,-20,-20,32,32,-32,-32,23,23,-23,-23,30,30,-30,-30,8,8,-8,-8,37,37,-37,-37,12,12,-12,-12,36,36,-36,-36,0,0,38,-38,1,1,-1,-1,17,17,-17,-17,22,22,-22,-22,31,31,-31,-31,34,34,-34,-34,38,38,-38,-38,2,2,-2,-2,38,38,-38,-38,9,9,-9,-9,15,15,-15,-15,19,19,-19,-19,33,33,-33,-33,35,35,-35,-35,37,37,-37,-37,3,3,-3,-3,38,38,-38,-38,27,27,-27,-27,4,4,-4,-4,26,26,-26,-26,28,28,-28,-28,38,38,-38,-38,13,13,-13,-13,21,21,-21,-21,32,32,-32,-32,36,36,-36,-36,25,25,-25,-25,29,29,-29,-29,5,5,-5,-5,10,10,-10,-10,37,37,-37,-37,38,38,-38,-38,24,24,-24,-24,30,30,-30,-30,6,6,-6,-6,18,18,-18,-18,34,34,-34,-34,38,38,-38,-38,16,16,-16,-16,35,35,-35,-35,20,20,-20,-20,33,33,-33,-33,11,11,-11,-11,23,23,-23,-23,31,31,-31,-31,37,37,-37,-37,14,14,-14,-14,36,36,-36,-36,7,7,-7,-7,38,38,-38,-38,8,8,-8,-8,22,22,-22,-22,32,32,-32,-32,38,38,-38,-38,12,12,-12,-12,27,27,-27,-27,28,28,-28,-28,37,37,-37,-37,17,17,-17,-17,35,35,-35,-35,19,19,-19,-19,26,26,-26,-26,29,29,-29,-29,34,34,-34,-34,0,0,15,15,-15,-15,36,36,-36,-36,39,-39,1,1,-1,-1,39,39,-39,-39,2,2,-2,-2,9,9,-9,-9,25,25,-25,-25,30,30,-30,-30,38,38,-38,-38,39,39,-39,-39,3,3,-3,-3,21,21,-21,-21,33,33,-33,-33,39,39,-39,-39,4,4,-4,-4,24,24,-24,-24,31,31,-31,-31,39,39,-39,-39,13,13,-13,-13,37,37,-37,-37,10,10,-10,-10,38,38,-38,-38,5,5,-5,-5,39,39,-39,-39,18,18,-18,-18,35,35,-35,-35,16,16,-16,-16,36,36,-36,-36,23,23,-23,-23,32,32,-32,-32,20,20,-20,-20,34,34,-34,-34,6,6,-6,-6,39,39,-39,-39,11,11,-11,-11,14,14,-14,-14,37,37,-37,-37,38,38,-38,-38,28,28,-28,-28,7,7,-7,-7,27,27,-27,-27,29,29,-29,-29,39,39,-39,-39,22,22,-22,-22,33,33,-33,-33,26,26,-26,-26,30,30,-30,-30,8,8,-8,-8,17,17,-17,-17,36,36,-36,-36,39,39,-39,-39,19,19,-19,-19,25,25,-25,-25,31,31,-31,-31,35,35,-35,-35,12,12,-12,-12,38,38,-38,-38,15,15,-15,-15,37,37,-37,-37,21,21,-21,-21,34,34,-34,-34,0,0,24,24,-24,-24,32,32,-32,-32,40,-40,1,1,-1,-1,40,40,-40,-40,9,9,-9,-9,39,39,-39,-39,2,2,-2,-2,40,40,-40,-40,3,3,-3,-3,40,40,-40,-40,13,13,-13,-13,38,38,-38,-38,4,4,-4,-4,40,40,-40,-40,23,23,-23,-23,33,33,-33,-33,18,18,-18,-18,36,36,-36,-36,10,10,-10,-10,39,39,-39,-39,5,5,-5,-5,16,16,-16,-16,20,20,-20,-20,28,28,-28,-28,29,29,-29,-29,35,35,-35,-35,37,37,-37,-37,40,40,-40,-40,27,27,-27,-27,30,30,-30,-30,6,6,-6,-6,40,40,-40,-40,26,26,-26,-26,31,31,-31,-31,14,14,-14,-14,22,22,-22,-22,34,34,-34,-34,38,38,-38,-38,11,11,-11,-11,39,39,-39,-39,7,7,-7,-7,25,25,-25,-25,32,32,-32,-32,40,40,-40,-40,19,19,-19,-19,36,36,-36,-36,17,17,-17,-17,37,37,-37,-37,8,8,-8,-8,40,40,-40,-40,12,12,-12,-12,24,24,-24,-24,33,33,-33,-33,39,39,-39,-39,21,21,-21,-21,35,35,-35,-35,15,15,-15,-15,38,38,-38,-38,0,0,9,9,-9,-9,40,40,-40,-40,41,-41,1,1,-1,-1,29,29,-29,-29,41,41,-41,-41,28,28,-28,-28,30,30,-30,-30,2,2,-2,-2,23,23,-23,-23,34,34,-34,-34,41,41,-41,-41,3,3,-3,-3,13,13,-13,-13,27,27,-27,-27,31,31,-31,-31,39,39,-39,-39,41,41,-41,-41,18,18,-18,-18,37,37,-37,-37,20,20,-20,-20,36,36,-36,-36,4,4,-4,-4,41,41,-41,-41,10,10,-10,-10,16,16,-16,-16,26,26,-26,-26,32,32,-32,-32,38,38,-38,-38,40,40,-40,-40,5,5,-5,-5,41,41,-41,-41,22,22,-22,-22,35,35,-35,-35,25,25,-25,-25,33,33,-33,-33,6,6,-6,-6,14,14,-14,-14,39,39,-39,-39,41,41,-41,-41,11,11,-11,-11,40,40,-40,-40,7,7,-7,-7,19,19,-19,-19,37,37,-37,-37,41,41,-41,-41,24,24,-24,-24,34,34,-34,-34,17,17,-17,-17,38,38,-38,-38,21,21,-21,-21,36,36,-36,-36,29,29,-29,-29,30,30,-30,-30,12,12,-12,-12,40,40,-40,-40,8,8,-8,-8,28,28,-28,-28,31,31,-31,-31,41,41,-41,-41,15,15,-15,-15,39,39,-39,-39,27,27,-27,-27,32,32,-32,-32,23,23,-23,-23,35,35,-35,-35,9,9,-9,-9,41,41,-41,-41,0,0,42,-42,1,1,-1,-1,26,26,-26,-26,33,33,-33,-33,42,42,-42,-42,2,2,-2,-2,18,18,-18,-18,38,38,-38,-38,42,42,-42,-42,13,13,-13,-13,20,20,-20,-20,37,37,-37,-37,40,40,-40,-40,3,3,-3,-3,42,42,-42,-42,16,16,-16,-16,39,39,-39,-39,4,4,-4,-4,22,22,-22,-22,36,36,-36,-36,42,42,-42,-42,10,10,-10,-10,25,25,-25,-25,34,34,-34,-34,41,41,-41,-41,5,5,-5,-5,42,42,-42,-42,14,14,-14,-14,40,40,-40,-40,6,6,-6,-6,30,30,-30,-30,42,42,-42,-42,24,24,-24,-24,35,35,-35,-35,11,11,-11,-11,29,29,-29,-29,31,31,-31,-31,41,41,-41,-41,19,19,-19,-19,38,38,-38,-38,28,28,-28,-28,32,32,-32,-32,17,17,-17,-17,21,21,-21,-21,37,37,-37,-37,39,39,-39,-39,7,7,-7,-7,42,42,-42,-42,27,27,-27,-27,33,33,-33,-33,12,12,-12,-12,15,15,-15,-15,23,23,-23,-23,36,36,-36,-36,40,40,-40,-40,41,41,-41,-41,8,8,-8,-8,42,42,-42,-42,26,26,-26,-26,34,34,-34,-34,20,20,-20,-20,38,38,-38,-38,9,9,-9,-9,18,18,-18,-18,39,39,-39,-39,42,42,-42,-42,0,0,43,-43,1,1,-1,-1,13,13,-13,-13,25,25,-25,-25,35,35,-35,-35,41,41,-41,-41,43,43,-43,-43,2,2,-2,-2,22,22,-22,-22,37,37,-37,-37,43,43,-43,-43,16,16,-16,-16,40,40,-40,-40,3,3,-3,-3,43,43,-43,-43,30,30,-30,-30,31,31,-31,-31,10,10,-10,-10,42,42,-42,-42,4,4,-4,-4,29,29,-29,-29,32,32,-32,-32,43,43,-43,-43,24,24,-24,-24,36,36,-36,-36,28,28,-28,-28,33,33,-33,-33,5,5,-5,-5,43,43,-43,-43,14,14,-14,-14,41,41,-41,-41,19,19,-19,-19,39,39,-39,-39,6,6,-6,-6,11,11,-11,-11,21,21,-21,-21,27,27,-27,-27,34,34,-34,-34,38,38,-38,-38,42,42,-42,-42,43,43,-43,-43,17,17,-17,-17,40,40,-40,-40,7,7,-7,-7,23,23,-23,-23,37,37,-37,-37,43,43,-43,-43,26,26,-26,-26,35,35,-35,-35,15,15,-15,-15,41,41,-41,-41,12,12,-12,-12,42,42,-42,-42,8,8,-8,-8,43,43,-43,-43,20,20,-20,-20,25,25,-25,-25,36,36,-36,-36,39,39,-39,-39,31,31,-31,-31,18,18,-18,-18,30,30,-30,-30,32,32,-32,-32,40,40,-40,-40,22,22,-22,-22,38,38,-38,-38,9,9,-9,-9,29,29,-29,-29,33,33,-33,-33,43,43,-43,-43,13,13,-13,-13,42,42,-42,-42,0,0,44,-44,1,1,-1,-1,16,16,-16,-16,41,41,-41,-41,44,44,-44,-44,2,2,-2,-2,28,28,-28,-28,34,34,-34,-34,44,44,-44,-44,3,3,-3,-3,24,24,-24,-24,37,37,-37,-37,44,44,-44,-44,10,10,-10,-10,43,43,-43,-43,4,4,-4,-4,44,44,-44,-44,27,27,-27,-27,35,35,-35,-35,14,14,-14,-14,42,42,-42,-42,5,5,-5,-5,19,19,-19,-19,40,40,-40,-40,44,44,-44,-44,21,21,-21,-21,39,39,-39,-39,11,11,-11,-11,17,17,-17,-17,41,41,-41,-41,43,43,-43,-43,6,6,-6,-6,26,26,-26,-26,36,36,-36,-36,44,44,-44,-44,23,23,-23,-23,38,38,-38,-38,7,7,-7,-7,31,31,-31,-31,32,32,-32,-32,44,44,-44,-44,15,15,-15,-15,30,30,-30,-30,33,33,-33,-33,42,42,-42,-42,12,12,-12,-12,43,43,-43,-43,25,25,-25,-25,37,37,-37,-37,29,29,-29,-29,34,34,-34,-34,8,8,-8,-8,20,20,-20,-20,40,40,-40,-40,44,44,-44,-44,18,18,-18,-18,22,22,-22,-22,39,39,-39,-39,41,41,-41,-41,28,28,-28,-28,35,35,-35,-35,9,9,-9,-9,44,44,-44,-44,13,13,-13,-13,43,43,-43,-43,16,16,-16,-16,24,24,-24,-24,38,38,-38,-38,42,42,-42,-42,0,0,27,27,-27,-27,36,36,-36,-36,45,-45,1,1,-1,-1,45,45,-45,-45,2,2,-2,-2,45,45,-45,-45,3,3,-3,-3,45,45,-45,-45,10,10,-10,-10,44,44,-44,-44,4,4,-4,-4,21,21,-21,-21,40,40,-40,-40,45,45,-45,-45,19,19,-19,-19,41,41,-41,-41,14,14,-14,-14,26,26,-26,-26,37,37,-37,-37,43,43,-43,-43,32,32,-32,-32,5,5,-5,-5,23,23,-23,-23,31,31,-31,-31,33,33,-33,-33,39,39,-39,-39,45,45,-45,-45,17,17,-17,-17,42,42,-42,-42,30,30,-30,-30,34,34,-34,-34,11,11,-11,-11,44,44,-44,-44,6,6,-6,-6,45,45,-45,-45,29,29,-29,-29,35,35,-35,-35,25,25,-25,-25,38,38,-38,-38,7,7,-7,-7,15,15,-15,-15,43,43,-43,-43,45,45,-45,-45,12,12,-12,-12,28,28,-28,-28,36,36,-36,-36,44,44,-44,-44,20,20,-20,-20,41,41,-41,-41,22,22,-22,-22,40,40,-40,-40,18,18,-18,-18,42,42,-42,-42,8,8,-8,-8,45,45,-45,-45,24,24,-24,-24,39,39,-39,-39,27,27,-27,-27,37,37,-37,-37,13,13,-13,-13,16,16,-16,-16,43,43,-43,-43,44,44,-44,-44,9,9,-9,-9,45,45,-45,-45,32,32,-32,-32,33,33,-33,-33,0,0,46,-46,1,1,-1,-1,31,31,-31,-31,34,34,-34,-34,46,46,-46,-46,2,2,-2,-2,26,26,-26,-26,38,38,-38,-38,46,46,-46,-46,21,21,-21,-21,41,41,-41,-41,3,3,-3,-3,10,10,-10,-10,19,19,-19,-19,30,30,-30,-30,35,35,-35,-35,42,42,-42,-42,45,45,-45,-45,46,46,-46,-46,23,23,-23,-23,40,40,-40,-40,4,4,-4,-4,14,14,-14,-14,44,44,-44,-44,46,46,-46,-46,29,29,-29,-29,36,36,-36,-36,17,17,-17,-17,43,43,-43,-43,5,5,-5,-5,46,46,-46,-46,11,11,-11,-11,25,25,-25,-25,39,39,-39,-39,45,45,-45,-45,6,6,-6,-6,46,46,-46,-46,28,28,-28,-28,37,37,-37,-37,15,15,-15,-15,44,44,-44,-44,20,20,-20,-20,42,42,-42,-42,7,7,-7,-7,22,22,-22,-22,41,41,-41,-41,46,46,-46,-46,12,12,-12,-12,45,45,-45,-45,18,18,-18,-18,27,27,-27,-27,38,38,-38,-38,43,43,-43,-43,24,24,-24,-24,40,40,-40,-40,33,33,-33,-33,8,8,-8,-8,32,32,-32,-32,34,34,-34,-34,46,46,-46,-46,31,31,-31,-31,35,35,-35,-35,16,16,-16,-16,44,44,-44,-44,13,13,-13,-13,45,45,-45,-45,30,30,-30,-30,36,36,-36,-36,9,9,-9,-9,26,26,-26,-26,39,39,-39,-39,46,46,-46,-46,21,21,-21,-21,42,42,-42,-42,0,0,47,-47,1,1,-1,-1,19,19,-19,-19,23,23,-23,-23,29,29,-29,-29,37,37,-37,-37,41,41,-41,-41,43,43,-43,-43,47,47,-47,-47,2,2,-2,-2,47,47,-47,-47,10,10,-10,-10,46,46,-46,-46,3,3,-3,-3,47,47,-47,-47,14,14,-14,-14,45,45,-45,-45,4,4,-4,-4,17,17,-17,-17,25,25,-25,-25,40,40,-40,-40,44,44,-44,-44,47,47,-47,-47,28,28,-28,-28,38,38,-38,-38,5,5,-5,-5,47,47,-47,-47,11,11,-11,-11,46,46,-46,-46,6,6,-6,-6,33,33,-33,-33,34,34,-34,-34,47,47,-47,-47,22,22,-22,-22,42,42,-42,-42,20,20,-20,-20,32,32,-32,-32,35,35,-35,-35,43,43,-43,-43,15,15,-15,-15,27,27,-27,-27,39,39,-39,-39,45,45,-45,-45,24,24,-24,-24,31,31,-31,-31,36,36,-36,-36,41,41,-41,-41,7,7,-7,-7,47,47,-47,-47,12,12,-12,-12,18,18,-18,-18,44,44,-44,-44,46,46,-46,-46,30,30,-30,-30,37,37,-37,-37,8,8,-8,-8,47,47,-47,-47,26,26,-26,-26,40,40,-40,-40,16,16,-16,-16,45,45,-45,-45,13,13,-13,-13,29,29,-29,-29,38,38,-38,-38,46,46,-46,-46,9,9,-9,-9,21,21,-21,-21,43,43,-43,-43,47,47,-47,-47,23,23,-23,-23,42,42,-42,-42,19,19,-19,-19,44,44,-44,-44,0,0,48,-48,1,1,-1,-1,28,28,-28,-28,39,39,-39,-39,48,48,-48,-48,25,25,-25,-25,41,41,-41,-41,2,2,-2,-2,48,48,-48,-48,10,10,-10,-10,47,47,-47,-47,14,14,-14,-14,34,34,-34,-34,46,46,-46,-46,3,3,-3,-3,48,48,-48,-48,17,17,-17,-17,33,33,-33,-33,35,35,-35,-35,45,45,-45,-45,4,4,-4,-4,32,32,-32,-32,36,36,-36,-36,48,48,-48,-48,5,5,-5,-5,27,27,-27,-27,40,40,-40,-40,48,48,-48,-48,11,11,-11,-11,31,31,-31,-31,37,37,-37,-37,47,47,-47,-47,22,22,-22,-22,43,43,-43,-43,20,20,-20,-20,44,44,-44,-44,6,6,-6,-6,24,24,-24,-24,42,42,-42,-42,48,48,-48,-48,15,15,-15,-15,46,46,-46,-46,30,30,-30,-30,38,38,-38,-38,18,18,-18,-18,45,45,-45,-45,7,7,-7,-7,12,12,-12,-12,47,47,-47,-47,48,48,-48,-48,26,26,-26,-26,41,41,-41,-41,29,29,-29,-29,39,39,-39,-39,8,8,-8,-8,48,48,-48,-48,16,16,-16,-16,46,46,-46,-46,21,21,-21,-21,44,44,-44,-44,13,13,-13,-13,23,23,-23,-23,43,43,-43,-43,47,47,-47,-47,34,34,-34,-34,35,35,-35,-35,28,28,-28,-28,40,40,-40,-40,9,9,-9,-9,33,33,-33,-33,36,36,-36,-36,48,48,-48,-48,19,19,-19,-19,45,45,-45,-45,25,25,-25,-25,42,42,-42,-42,32,32,-32,-32,37,37,-37,-37,0,0,49,-49,1,1,-1,-1,49,49,-49,-49,10,10,-10,-10,48,48,-48,-48,2,2,-2,-2,14,14,-14,-14,17,17,-17,-17,31,31,-31,-31,38,38,-38,-38,46,46,-46,-46,47,47,-47,-47,49,49,-49,-49,3,3,-3,-3,27,27,-27,-27,41,41,-41,-41,49,49,-49,-49,4,4,-4,-4,49,49,-49,-49,22,22,-22,-22,44,44,-44,-44,30,30,-30,-30,39,39,-39,-39,11,11,-11,-11,20,20,-20,-20,24,24,-24,-24,43,43,-43,-43,45,45,-45,-45,48,48,-48,-48,5,5,-5,-5,49,49,-49,-49,15,15,-15,-15,47,47,-47,-47,6,6,-6,-6,49,49,-49,-49,18,18,-18,-18,26,26,-26,-26,42,42,-42,-42,46,46,-46,-46,29,29,-29,-29,40,40,-40,-40,12,12,-12,-12,48,48,-48,-48,7,7,-7,-7,35,35,-35,-35,49,49,-49,-49,34,34,-34,-34,36,36,-36,-36,33,33,-33,-33,37,37,-37,-37,8,8,-8,-8,16,16,-16,-16,23,23,-23,-23,28,28,-28,-28,41,41,-41,-41,44,44,-44,-44,47,47,-47,-47,49,49,-49,-49,21,21,-21,-21,45,45,-45,-45,32,32,-32,-32,38,38,-38,-38,13,13,-13,-13,48,48,-48,-48,25,25,-25,-25,43,43,-43,-43,19,19,-19,-19,46,46,-46,-46,9,9,-9,-9,31,31,-31,-31,39,39,-39,-39,49,49,-49,-49,27,27,-27,-27,42,42,-42,-42,17,17,-17,-17,47,47,-47,-47,0,0,14,14,-14,-14,30,30,-30,-30,40,40,-40,-40,48,48,-48,-48,50,-50}; + +const int expansion_pos_c[7845] = {0,1,-1,0,0,1,-1,1,-1,2,-2,0,0,2,-2,2,-2,1,-1,1,-1,2,-2,2,-2,3,-3,0,0,3,-3,3,-3,1,-1,1,-1,3,-3,3,-3,2,-2,2,-2,4,-4,0,0,4,-4,4,-4,1,-1,1,-1,3,-3,3,-3,4,-4,4,-4,2,-2,2,-2,5,-5,4,-4,4,-4,3,-3,3,-3,0,0,5,-5,5,-5,1,-1,1,-1,5,-5,5,-5,2,-2,2,-2,4,-4,4,-4,5,-5,5,-5,3,-3,3,-3,6,-6,0,0,6,-6,6,-6,1,-1,1,-1,6,-6,6,-6,2,-2,2,-2,5,-5,5,-5,4,-4,4,-4,6,-6,6,-6,3,-3,3,-3,7,-7,0,0,7,-7,7,-7,5,-5,5,-5,1,-1,1,-1,6,-6,6,-6,4,-4,4,-4,7,-7,7,-7,2,-2,2,-2,7,-7,7,-7,3,-3,3,-3,6,-6,6,-6,5,-5,5,-5,8,-8,0,0,8,-8,8,-8,7,-7,7,-7,4,-4,4,-4,1,-1,1,-1,8,-8,8,-8,2,-2,2,-2,6,-6,6,-6,8,-8,8,-8,3,-3,3,-3,7,-7,7,-7,5,-5,5,-5,8,-8,8,-8,4,-4,4,-4,9,-9,0,0,9,-9,9,-9,1,-1,1,-1,9,-9,9,-9,7,-7,7,-7,6,-6,6,-6,2,-2,2,-2,8,-8,8,-8,5,-5,5,-5,9,-9,9,-9,3,-3,3,-3,9,-9,9,-9,4,-4,4,-4,7,-7,7,-7,10,-10,8,-8,8,-8,6,-6,6,-6,0,0,10,-10,10,-10,1,-1,1,-1,10,-10,10,-10,2,-2,2,-2,9,-9,9,-9,5,-5,5,-5,10,-10,10,-10,3,-3,3,-3,8,-8,8,-8,7,-7,7,-7,10,-10,10,-10,4,-4,4,-4,9,-9,9,-9,6,-6,6,-6,11,-11,0,0,11,-11,11,-11,1,-1,1,-1,11,-11,11,-11,10,-10,10,-10,5,-5,5,-5,2,-2,2,-2,8,-8,8,-8,11,-11,11,-11,9,-9,9,-9,7,-7,7,-7,3,-3,3,-3,10,-10,10,-10,6,-6,6,-6,11,-11,11,-11,4,-4,4,-4,12,-12,0,0,12,-12,12,-12,9,-9,9,-9,8,-8,8,-8,1,-1,1,-1,11,-11,11,-11,5,-5,5,-5,12,-12,12,-12,2,-2,2,-2,10,-10,10,-10,7,-7,7,-7,12,-12,12,-12,3,-3,3,-3,11,-11,11,-11,6,-6,6,-6,12,-12,12,-12,4,-4,4,-4,9,-9,9,-9,10,-10,10,-10,8,-8,8,-8,13,-13,12,-12,12,-12,5,-5,5,-5,0,0,13,-13,13,-13,11,-11,11,-11,7,-7,7,-7,1,-1,1,-1,13,-13,13,-13,2,-2,2,-2,13,-13,13,-13,3,-3,3,-3,12,-12,12,-12,6,-6,6,-6,10,-10,10,-10,9,-9,9,-9,13,-13,13,-13,11,-11,11,-11,8,-8,8,-8,4,-4,4,-4,12,-12,12,-12,7,-7,7,-7,13,-13,13,-13,5,-5,5,-5,14,-14,0,0,14,-14,14,-14,1,-1,1,-1,14,-14,14,-14,10,-10,10,-10,2,-2,2,-2,11,-11,11,-11,9,-9,9,-9,14,-14,14,-14,13,-13,13,-13,6,-6,6,-6,3,-3,3,-3,12,-12,12,-12,8,-8,8,-8,14,-14,14,-14,4,-4,4,-4,13,-13,13,-13,7,-7,7,-7,14,-14,14,-14,11,-11,11,-11,10,-10,10,-10,5,-5,5,-5,15,-15,12,-12,12,-12,9,-9,9,-9,0,0,15,-15,15,-15,1,-1,1,-1,15,-15,15,-15,2,-2,2,-2,14,-14,14,-14,6,-6,6,-6,13,-13,13,-13,8,-8,8,-8,15,-15,15,-15,3,-3,3,-3,15,-15,15,-15,4,-4,4,-4,11,-11,11,-11,12,-12,12,-12,10,-10,10,-10,14,-14,14,-14,7,-7,7,-7,15,-15,15,-15,13,-13,13,-13,9,-9,9,-9,5,-5,5,-5,16,-16,0,0,16,-16,16,-16,1,-1,1,-1,16,-16,16,-16,14,-14,14,-14,8,-8,8,-8,2,-2,2,-2,15,-15,15,-15,6,-6,6,-6,16,-16,16,-16,12,-12,12,-12,11,-11,11,-11,3,-3,3,-3,13,-13,13,-13,10,-10,10,-10,16,-16,16,-16,4,-4,4,-4,15,-15,15,-15,7,-7,7,-7,14,-14,14,-14,9,-9,9,-9,16,-16,16,-16,5,-5,5,-5,12,-12,12,-12,17,-17,15,-15,15,-15,8,-8,8,-8,0,0,17,-17,17,-17,13,-13,13,-13,11,-11,11,-11,1,-1,1,-1,16,-16,16,-16,6,-6,6,-6,17,-17,17,-17,2,-2,2,-2,14,-14,14,-14,10,-10,10,-10,17,-17,17,-17,3,-3,3,-3,17,-17,17,-17,16,-16,16,-16,7,-7,7,-7,4,-4,4,-4,15,-15,15,-15,9,-9,9,-9,13,-13,13,-13,12,-12,12,-12,17,-17,17,-17,5,-5,5,-5,14,-14,14,-14,11,-11,11,-11,16,-16,16,-16,8,-8,8,-8,18,-18,0,0,18,-18,18,-18,17,-17,17,-17,15,-15,15,-15,10,-10,10,-10,6,-6,6,-6,1,-1,1,-1,18,-18,18,-18,2,-2,2,-2,18,-18,18,-18,3,-3,3,-3,16,-16,16,-16,9,-9,9,-9,17,-17,17,-17,13,-13,13,-13,7,-7,7,-7,18,-18,18,-18,14,-14,14,-14,12,-12,12,-12,4,-4,4,-4,15,-15,15,-15,11,-11,11,-11,18,-18,18,-18,5,-5,5,-5,17,-17,17,-17,8,-8,8,-8,16,-16,16,-16,10,-10,10,-10,18,-18,18,-18,6,-6,6,-6,19,-19,0,0,19,-19,19,-19,1,-1,1,-1,19,-19,19,-19,14,-14,14,-14,13,-13,13,-13,2,-2,2,-2,15,-15,15,-15,12,-12,12,-12,19,-19,19,-19,17,-17,17,-17,9,-9,9,-9,3,-3,3,-3,18,-18,18,-18,7,-7,7,-7,19,-19,19,-19,16,-16,16,-16,11,-11,11,-11,4,-4,4,-4,19,-19,19,-19,5,-5,5,-5,18,-18,18,-18,8,-8,8,-8,17,-17,17,-17,10,-10,10,-10,14,-14,14,-14,15,-15,15,-15,13,-13,13,-13,19,-19,19,-19,6,-6,6,-6,20,-20,16,-16,16,-16,12,-12,12,-12,0,0,20,-20,20,-20,1,-1,1,-1,20,-20,20,-20,2,-2,2,-2,18,-18,18,-18,9,-9,9,-9,20,-20,20,-20,3,-3,3,-3,19,-19,19,-19,17,-17,17,-17,11,-11,11,-11,7,-7,7,-7,20,-20,20,-20,4,-4,4,-4,15,-15,15,-15,14,-14,14,-14,18,-18,18,-18,10,-10,10,-10,20,-20,20,-20,19,-19,19,-19,16,-16,16,-16,13,-13,13,-13,8,-8,8,-8,5,-5,5,-5,17,-17,17,-17,12,-12,12,-12,20,-20,20,-20,6,-6,6,-6,21,-21,0,0,21,-21,21,-21,19,-19,19,-19,9,-9,9,-9,1,-1,1,-1,21,-21,21,-21,18,-18,18,-18,11,-11,11,-11,2,-2,2,-2,20,-20,20,-20,7,-7,7,-7,21,-21,21,-21,15,-15,15,-15,3,-3,3,-3,16,-16,16,-16,14,-14,14,-14,21,-21,21,-21,4,-4,4,-4,17,-17,17,-17,13,-13,13,-13,19,-19,19,-19,10,-10,10,-10,20,-20,20,-20,8,-8,8,-8,21,-21,21,-21,5,-5,5,-5,18,-18,18,-18,12,-12,12,-12,21,-21,21,-21,6,-6,6,-6,20,-20,20,-20,16,-16,16,-16,15,-15,15,-15,9,-9,9,-9,19,-19,19,-19,11,-11,11,-11,22,-22,0,0,22,-22,22,-22,17,-17,17,-17,14,-14,14,-14,1,-1,1,-1,22,-22,22,-22,2,-2,2,-2,21,-21,21,-21,7,-7,7,-7,22,-22,22,-22,18,-18,18,-18,13,-13,13,-13,3,-3,3,-3,22,-22,22,-22,20,-20,20,-20,10,-10,10,-10,4,-4,4,-4,21,-21,21,-21,19,-19,19,-19,12,-12,12,-12,8,-8,8,-8,22,-22,22,-22,5,-5,5,-5,16,-16,16,-16,17,-17,17,-17,15,-15,15,-15,22,-22,22,-22,18,-18,18,-18,14,-14,14,-14,6,-6,6,-6,20,-20,20,-20,11,-11,11,-11,21,-21,21,-21,9,-9,9,-9,23,-23,0,0,23,-23,23,-23,19,-19,19,-19,13,-13,13,-13,1,-1,1,-1,23,-23,23,-23,22,-22,22,-22,7,-7,7,-7,2,-2,2,-2,23,-23,23,-23,3,-3,3,-3,21,-21,21,-21,10,-10,10,-10,20,-20,20,-20,12,-12,12,-12,23,-23,23,-23,17,-17,17,-17,16,-16,16,-16,4,-4,4,-4,22,-22,22,-22,8,-8,8,-8,18,-18,18,-18,15,-15,15,-15,23,-23,23,-23,5,-5,5,-5,19,-19,19,-19,14,-14,14,-14,21,-21,21,-21,11,-11,11,-11,23,-23,23,-23,22,-22,22,-22,9,-9,9,-9,6,-6,6,-6,20,-20,20,-20,13,-13,13,-13,24,-24,0,0,24,-24,24,-24,1,-1,1,-1,23,-23,23,-23,17,-17,17,-17,7,-7,7,-7,24,-24,24,-24,18,-18,18,-18,16,-16,16,-16,2,-2,2,-2,22,-22,22,-22,10,-10,10,-10,24,-24,24,-24,21,-21,21,-21,12,-12,12,-12,3,-3,3,-3,19,-19,19,-19,15,-15,15,-15,24,-24,24,-24,4,-4,4,-4,23,-23,23,-23,8,-8,8,-8,20,-20,20,-20,14,-14,14,-14,24,-24,24,-24,5,-5,5,-5,22,-22,22,-22,11,-11,11,-11,23,-23,23,-23,21,-21,21,-21,13,-13,13,-13,9,-9,9,-9,24,-24,24,-24,6,-6,6,-6,18,-18,18,-18,17,-17,17,-17,19,-19,19,-19,16,-16,16,-16,25,-25,24,-24,24,-24,20,-20,20,-20,15,-15,15,-15,7,-7,7,-7,0,0,25,-25,25,-25,1,-1,1,-1,22,-22,22,-22,12,-12,12,-12,25,-25,25,-25,23,-23,23,-23,10,-10,10,-10,2,-2,2,-2,25,-25,25,-25,3,-3,3,-3,21,-21,21,-21,14,-14,14,-14,24,-24,24,-24,8,-8,8,-8,25,-25,25,-25,4,-4,4,-4,18,-18,18,-18,25,-25,25,-25,23,-23,23,-23,19,-19,19,-19,17,-17,17,-17,11,-11,11,-11,5,-5,5,-5,22,-22,22,-22,13,-13,13,-13,20,-20,20,-20,16,-16,16,-16,24,-24,24,-24,9,-9,9,-9,25,-25,25,-25,6,-6,6,-6,21,-21,21,-21,15,-15,15,-15,23,-23,23,-23,12,-12,12,-12,25,-25,25,-25,7,-7,7,-7,26,-26,24,-24,24,-24,10,-10,10,-10,0,0,26,-26,26,-26,1,-1,1,-1,26,-26,26,-26,22,-22,22,-22,14,-14,14,-14,2,-2,2,-2,26,-26,26,-26,19,-19,19,-19,18,-18,18,-18,3,-3,3,-3,25,-25,25,-25,20,-20,20,-20,17,-17,17,-17,8,-8,8,-8,26,-26,26,-26,4,-4,4,-4,24,-24,24,-24,21,-21,21,-21,16,-16,16,-16,11,-11,11,-11,23,-23,23,-23,13,-13,13,-13,26,-26,26,-26,5,-5,5,-5,25,-25,25,-25,9,-9,9,-9,22,-22,22,-22,15,-15,15,-15,26,-26,26,-26,6,-6,6,-6,24,-24,24,-24,12,-12,12,-12,19,-19,19,-19,20,-20,20,-20,18,-18,18,-18,26,-26,26,-26,25,-25,25,-25,23,-23,23,-23,14,-14,14,-14,10,-10,10,-10,7,-7,7,-7,27,-27,0,0,27,-27,27,-27,21,-21,21,-21,17,-17,17,-17,1,-1,1,-1,27,-27,27,-27,2,-2,2,-2,27,-27,27,-27,3,-3,3,-3,26,-26,26,-26,22,-22,22,-22,16,-16,16,-16,8,-8,8,-8,27,-27,27,-27,24,-24,24,-24,13,-13,13,-13,4,-4,4,-4,25,-25,25,-25,11,-11,11,-11,27,-27,27,-27,23,-23,23,-23,15,-15,15,-15,5,-5,5,-5,26,-26,26,-26,9,-9,9,-9,20,-20,20,-20,19,-19,19,-19,27,-27,27,-27,21,-21,21,-21,18,-18,18,-18,6,-6,6,-6,25,-25,25,-25,12,-12,12,-12,24,-24,24,-24,14,-14,14,-14,22,-22,22,-22,17,-17,17,-17,26,-26,26,-26,10,-10,10,-10,27,-27,27,-27,7,-7,7,-7,28,-28,0,0,28,-28,28,-28,23,-23,23,-23,16,-16,16,-16,1,-1,1,-1,28,-28,28,-28,2,-2,2,-2,28,-28,28,-28,27,-27,27,-27,8,-8,8,-8,3,-3,3,-3,25,-25,25,-25,13,-13,13,-13,26,-26,26,-26,11,-11,11,-11,28,-28,28,-28,20,-20,20,-20,4,-4,4,-4,24,-24,24,-24,15,-15,15,-15,21,-21,21,-21,19,-19,19,-19,22,-22,22,-22,18,-18,18,-18,28,-28,28,-28,5,-5,5,-5,27,-27,27,-27,9,-9,9,-9,23,-23,23,-23,17,-17,17,-17,28,-28,28,-28,26,-26,26,-26,12,-12,12,-12,6,-6,6,-6,25,-25,25,-25,14,-14,14,-14,27,-27,27,-27,10,-10,10,-10,24,-24,24,-24,16,-16,16,-16,28,-28,28,-28,7,-7,7,-7,29,-29,21,-21,21,-21,20,-20,20,-20,0,0,29,-29,29,-29,1,-1,1,-1,29,-29,29,-29,26,-26,26,-26,22,-22,22,-22,19,-19,19,-19,13,-13,13,-13,2,-2,2,-2,28,-28,28,-28,8,-8,8,-8,29,-29,29,-29,27,-27,27,-27,25,-25,25,-25,15,-15,15,-15,11,-11,11,-11,3,-3,3,-3,23,-23,23,-23,18,-18,18,-18,29,-29,29,-29,4,-4,4,-4,28,-28,28,-28,24,-24,24,-24,17,-17,17,-17,9,-9,9,-9,29,-29,29,-29,5,-5,5,-5,26,-26,26,-26,14,-14,14,-14,27,-27,27,-27,12,-12,12,-12,29,-29,29,-29,6,-6,6,-6,25,-25,25,-25,16,-16,16,-16,21,-21,21,-21,28,-28,28,-28,22,-22,22,-22,20,-20,20,-20,10,-10,10,-10,29,-29,29,-29,23,-23,23,-23,19,-19,19,-19,7,-7,7,-7,27,-27,27,-27,13,-13,13,-13,30,-30,24,-24,24,-24,18,-18,18,-18,0,0,30,-30,30,-30,26,-26,26,-26,15,-15,15,-15,1,-1,1,-1,30,-30,30,-30,2,-2,2,-2,29,-29,29,-29,28,-28,28,-28,11,-11,11,-11,8,-8,8,-8,30,-30,30,-30,3,-3,3,-3,25,-25,25,-25,17,-17,17,-17,30,-30,30,-30,4,-4,4,-4,29,-29,29,-29,9,-9,9,-9,30,-30,30,-30,27,-27,27,-27,22,-22,22,-22,21,-21,21,-21,14,-14,14,-14,5,-5,5,-5,28,-28,28,-28,12,-12,12,-12,23,-23,23,-23,20,-20,20,-20,26,-26,26,-26,16,-16,16,-16,30,-30,30,-30,6,-6,6,-6,24,-24,24,-24,19,-19,19,-19,29,-29,29,-29,10,-10,10,-10,30,-30,30,-30,25,-25,25,-25,18,-18,18,-18,7,-7,7,-7,28,-28,28,-28,13,-13,13,-13,27,-27,27,-27,15,-15,15,-15,31,-31,0,0,31,-31,31,-31,29,-29,29,-29,11,-11,11,-11,1,-1,1,-1,30,-30,30,-30,8,-8,8,-8,31,-31,31,-31,26,-26,26,-26,17,-17,17,-17,2,-2,2,-2,22,-22,22,-22,31,-31,31,-31,23,-23,23,-23,21,-21,21,-21,3,-3,3,-3,24,-24,24,-24,20,-20,20,-20,31,-31,31,-31,4,-4,4,-4,28,-28,28,-28,14,-14,14,-14,30,-30,30,-30,9,-9,9,-9,29,-29,29,-29,27,-27,27,-27,16,-16,16,-16,12,-12,12,-12,31,-31,31,-31,25,-25,25,-25,19,-19,19,-19,5,-5,5,-5,31,-31,31,-31,6,-6,6,-6,30,-30,30,-30,26,-26,26,-26,18,-18,18,-18,10,-10,10,-10,28,-28,28,-28,15,-15,15,-15,31,-31,31,-31,29,-29,29,-29,13,-13,13,-13,7,-7,7,-7,23,-23,23,-23,22,-22,22,-22,24,-24,24,-24,21,-21,21,-21,27,-27,27,-27,17,-17,17,-17,30,-30,30,-30,11,-11,11,-11,32,-32,0,0,32,-32,32,-32,31,-31,31,-31,25,-25,25,-25,20,-20,20,-20,8,-8,8,-8,1,-1,1,-1,32,-32,32,-32,2,-2,2,-2,32,-32,32,-32,3,-3,3,-3,29,-29,29,-29,26,-26,26,-26,19,-19,19,-19,14,-14,14,-14,32,-32,32,-32,28,-28,28,-28,16,-16,16,-16,4,-4,4,-4,31,-31,31,-31,9,-9,9,-9,30,-30,30,-30,12,-12,12,-12,32,-32,32,-32,5,-5,5,-5,27,-27,27,-27,18,-18,18,-18,23,-23,23,-23,32,-32,32,-32,24,-24,24,-24,22,-22,22,-22,6,-6,6,-6,31,-31,31,-31,10,-10,10,-10,29,-29,29,-29,25,-25,25,-25,21,-21,21,-21,15,-15,15,-15,30,-30,30,-30,13,-13,13,-13,32,-32,32,-32,28,-28,28,-28,17,-17,17,-17,7,-7,7,-7,26,-26,26,-26,20,-20,20,-20,31,-31,31,-31,11,-11,11,-11,32,-32,32,-32,8,-8,8,-8,33,-33,0,0,33,-33,33,-33,27,-27,27,-27,19,-19,19,-19,1,-1,1,-1,33,-33,33,-33,2,-2,2,-2,30,-30,30,-30,14,-14,14,-14,29,-29,29,-29,16,-16,16,-16,33,-33,33,-33,3,-3,3,-3,33,-33,33,-33,32,-32,32,-32,31,-31,31,-31,24,-24,24,-24,23,-23,23,-23,12,-12,12,-12,9,-9,9,-9,4,-4,4,-4,28,-28,28,-28,18,-18,18,-18,25,-25,25,-25,22,-22,22,-22,33,-33,33,-33,5,-5,5,-5,26,-26,26,-26,21,-21,21,-21,32,-32,32,-32,10,-10,10,-10,33,-33,33,-33,30,-30,30,-30,15,-15,15,-15,6,-6,6,-6,27,-27,27,-27,20,-20,20,-20,31,-31,31,-31,29,-29,29,-29,17,-17,17,-17,13,-13,13,-13,33,-33,33,-33,7,-7,7,-7,32,-32,32,-32,28,-28,28,-28,19,-19,19,-19,11,-11,11,-11,24,-24,24,-24,33,-33,33,-33,8,-8,8,-8,25,-25,25,-25,23,-23,23,-23,34,-34,30,-30,30,-30,16,-16,16,-16,0,0,34,-34,34,-34,31,-31,31,-31,14,-14,14,-14,1,-1,1,-1,34,-34,34,-34,26,-26,26,-26,22,-22,22,-22,2,-2,2,-2,34,-34,34,-34,29,-29,29,-29,18,-18,18,-18,3,-3,3,-3,32,-32,32,-32,12,-12,12,-12,33,-33,33,-33,27,-27,27,-27,21,-21,21,-21,9,-9,9,-9,34,-34,34,-34,4,-4,4,-4,34,-34,34,-34,5,-5,5,-5,28,-28,28,-28,20,-20,20,-20,31,-31,31,-31,15,-15,15,-15,33,-33,33,-33,30,-30,30,-30,17,-17,17,-17,10,-10,10,-10,34,-34,34,-34,6,-6,6,-6,32,-32,32,-32,13,-13,13,-13,25,-25,25,-25,24,-24,24,-24,29,-29,29,-29,19,-19,19,-19,34,-34,34,-34,26,-26,26,-26,23,-23,23,-23,7,-7,7,-7,33,-33,33,-33,11,-11,11,-11,27,-27,27,-27,22,-22,22,-22,31,-31,31,-31,16,-16,16,-16,34,-34,34,-34,32,-32,32,-32,14,-14,14,-14,8,-8,8,-8,30,-30,30,-30,18,-18,18,-18,35,-35,28,-28,28,-28,21,-21,21,-21,0,0,35,-35,35,-35,1,-1,1,-1,35,-35,35,-35,2,-2,2,-2,33,-33,33,-33,12,-12,12,-12,35,-35,35,-35,3,-3,3,-3,34,-34,34,-34,9,-9,9,-9,35,-35,35,-35,29,-29,29,-29,20,-20,20,-20,4,-4,4,-4,32,-32,32,-32,15,-15,15,-15,35,-35,35,-35,31,-31,31,-31,25,-25,25,-25,17,-17,17,-17,5,-5,5,-5,26,-26,26,-26,24,-24,24,-24,34,-34,34,-34,10,-10,10,-10,33,-33,33,-33,27,-27,27,-27,23,-23,23,-23,13,-13,13,-13,35,-35,35,-35,30,-30,30,-30,19,-19,19,-19,6,-6,6,-6,28,-28,28,-28,22,-22,22,-22,35,-35,35,-35,7,-7,7,-7,34,-34,34,-34,11,-11,11,-11,32,-32,32,-32,16,-16,16,-16,29,-29,29,-29,21,-21,21,-21,33,-33,33,-33,31,-31,31,-31,18,-18,18,-18,14,-14,14,-14,35,-35,35,-35,8,-8,8,-8,36,-36,0,0,36,-36,36,-36,1,-1,1,-1,36,-36,36,-36,34,-34,34,-34,30,-30,30,-30,20,-20,20,-20,12,-12,12,-12,2,-2,2,-2,26,-26,26,-26,25,-25,25,-25,36,-36,36,-36,27,-27,27,-27,24,-24,24,-24,3,-3,3,-3,35,-35,35,-35,9,-9,9,-9,36,-36,36,-36,4,-4,4,-4,32,-32,32,-32,28,-28,28,-28,23,-23,23,-23,17,-17,17,-17,33,-33,33,-33,15,-15,15,-15,36,-36,36,-36,5,-5,5,-5,31,-31,31,-31,19,-19,19,-19,35,-35,35,-35,34,-34,34,-34,29,-29,29,-29,22,-22,22,-22,13,-13,13,-13,10,-10,10,-10,36,-36,36,-36,6,-6,6,-6,30,-30,30,-30,21,-21,21,-21,36,-36,36,-36,33,-33,33,-33,16,-16,16,-16,7,-7,7,-7,35,-35,35,-35,11,-11,11,-11,32,-32,32,-32,18,-18,18,-18,34,-34,34,-34,26,-26,26,-26,14,-14,14,-14,27,-27,27,-27,25,-25,25,-25,36,-36,36,-36,28,-28,28,-28,24,-24,24,-24,8,-8,8,-8,31,-31,31,-31,20,-20,20,-20,37,-37,35,-35,35,-35,12,-12,12,-12,0,0,37,-37,37,-37,29,-29,29,-29,23,-23,23,-23,1,-1,1,-1,37,-37,37,-37,2,-2,2,-2,36,-36,36,-36,9,-9,9,-9,37,-37,37,-37,33,-33,33,-33,17,-17,17,-17,3,-3,3,-3,34,-34,34,-34,15,-15,15,-15,30,-30,30,-30,22,-22,22,-22,37,-37,37,-37,32,-32,32,-32,19,-19,19,-19,4,-4,4,-4,37,-37,37,-37,35,-35,35,-35,13,-13,13,-13,5,-5,5,-5,36,-36,36,-36,10,-10,10,-10,31,-31,31,-31,21,-21,21,-21,37,-37,37,-37,27,-27,27,-27,26,-26,26,-26,6,-6,6,-6,28,-28,28,-28,25,-25,25,-25,34,-34,34,-34,16,-16,16,-16,33,-33,33,-33,18,-18,18,-18,36,-36,36,-36,29,-29,29,-29,24,-24,24,-24,11,-11,11,-11,37,-37,37,-37,7,-7,7,-7,35,-35,35,-35,14,-14,14,-14,32,-32,32,-32,20,-20,20,-20,30,-30,30,-30,23,-23,23,-23,37,-37,37,-37,8,-8,8,-8,36,-36,36,-36,12,-12,12,-12,38,-38,0,0,38,-38,38,-38,34,-34,34,-34,31,-31,31,-31,22,-22,22,-22,17,-17,17,-17,1,-1,1,-1,38,-38,38,-38,2,-2,2,-2,37,-37,37,-37,35,-35,35,-35,33,-33,33,-33,19,-19,19,-19,15,-15,15,-15,9,-9,9,-9,38,-38,38,-38,3,-3,3,-3,27,-27,27,-27,38,-38,38,-38,28,-28,28,-28,26,-26,26,-26,4,-4,4,-4,36,-36,36,-36,32,-32,32,-32,21,-21,21,-21,13,-13,13,-13,29,-29,29,-29,25,-25,25,-25,38,-38,38,-38,37,-37,37,-37,10,-10,10,-10,5,-5,5,-5,30,-30,30,-30,24,-24,24,-24,38,-38,38,-38,34,-34,34,-34,18,-18,18,-18,6,-6,6,-6,35,-35,35,-35,16,-16,16,-16,33,-33,33,-33,20,-20,20,-20,37,-37,37,-37,31,-31,31,-31,23,-23,23,-23,11,-11,11,-11,36,-36,36,-36,14,-14,14,-14,38,-38,38,-38,7,-7,7,-7,38,-38,38,-38,32,-32,32,-32,22,-22,22,-22,8,-8,8,-8,37,-37,37,-37,28,-28,28,-28,27,-27,27,-27,12,-12,12,-12,35,-35,35,-35,17,-17,17,-17,34,-34,34,-34,29,-29,29,-29,26,-26,26,-26,19,-19,19,-19,39,-39,36,-36,36,-36,15,-15,15,-15,0,0,39,-39,39,-39,1,-1,1,-1,39,-39,39,-39,38,-38,38,-38,30,-30,30,-30,25,-25,25,-25,9,-9,9,-9,2,-2,2,-2,39,-39,39,-39,33,-33,33,-33,21,-21,21,-21,3,-3,3,-3,39,-39,39,-39,31,-31,31,-31,24,-24,24,-24,4,-4,4,-4,37,-37,37,-37,13,-13,13,-13,38,-38,38,-38,10,-10,10,-10,39,-39,39,-39,5,-5,5,-5,35,-35,35,-35,18,-18,18,-18,36,-36,36,-36,16,-16,16,-16,32,-32,32,-32,23,-23,23,-23,34,-34,34,-34,20,-20,20,-20,39,-39,39,-39,6,-6,6,-6,38,-38,38,-38,37,-37,37,-37,14,-14,14,-14,11,-11,11,-11,28,-28,28,-28,39,-39,39,-39,29,-29,29,-29,27,-27,27,-27,7,-7,7,-7,33,-33,33,-33,22,-22,22,-22,30,-30,30,-30,26,-26,26,-26,39,-39,39,-39,36,-36,36,-36,17,-17,17,-17,8,-8,8,-8,35,-35,35,-35,31,-31,31,-31,25,-25,25,-25,19,-19,19,-19,38,-38,38,-38,12,-12,12,-12,37,-37,37,-37,15,-15,15,-15,34,-34,34,-34,21,-21,21,-21,40,-40,32,-32,32,-32,24,-24,24,-24,0,0,40,-40,40,-40,1,-1,1,-1,39,-39,39,-39,9,-9,9,-9,40,-40,40,-40,2,-2,2,-2,40,-40,40,-40,3,-3,3,-3,38,-38,38,-38,13,-13,13,-13,40,-40,40,-40,4,-4,4,-4,33,-33,33,-33,23,-23,23,-23,36,-36,36,-36,18,-18,18,-18,39,-39,39,-39,10,-10,10,-10,40,-40,40,-40,37,-37,37,-37,35,-35,35,-35,29,-29,29,-29,28,-28,28,-28,20,-20,20,-20,16,-16,16,-16,5,-5,5,-5,30,-30,30,-30,27,-27,27,-27,40,-40,40,-40,6,-6,6,-6,31,-31,31,-31,26,-26,26,-26,38,-38,38,-38,34,-34,34,-34,22,-22,22,-22,14,-14,14,-14,39,-39,39,-39,11,-11,11,-11,40,-40,40,-40,32,-32,32,-32,25,-25,25,-25,7,-7,7,-7,36,-36,36,-36,19,-19,19,-19,37,-37,37,-37,17,-17,17,-17,40,-40,40,-40,8,-8,8,-8,39,-39,39,-39,33,-33,33,-33,24,-24,24,-24,12,-12,12,-12,35,-35,35,-35,21,-21,21,-21,38,-38,38,-38,15,-15,15,-15,41,-41,40,-40,40,-40,9,-9,9,-9,0,0,41,-41,41,-41,29,-29,29,-29,1,-1,1,-1,30,-30,30,-30,28,-28,28,-28,41,-41,41,-41,34,-34,34,-34,23,-23,23,-23,2,-2,2,-2,41,-41,41,-41,39,-39,39,-39,31,-31,31,-31,27,-27,27,-27,13,-13,13,-13,3,-3,3,-3,37,-37,37,-37,18,-18,18,-18,36,-36,36,-36,20,-20,20,-20,41,-41,41,-41,4,-4,4,-4,40,-40,40,-40,38,-38,38,-38,32,-32,32,-32,26,-26,26,-26,16,-16,16,-16,10,-10,10,-10,41,-41,41,-41,5,-5,5,-5,35,-35,35,-35,22,-22,22,-22,33,-33,33,-33,25,-25,25,-25,41,-41,41,-41,39,-39,39,-39,14,-14,14,-14,6,-6,6,-6,40,-40,40,-40,11,-11,11,-11,41,-41,41,-41,37,-37,37,-37,19,-19,19,-19,7,-7,7,-7,34,-34,34,-34,24,-24,24,-24,38,-38,38,-38,17,-17,17,-17,36,-36,36,-36,21,-21,21,-21,30,-30,30,-30,29,-29,29,-29,40,-40,40,-40,12,-12,12,-12,41,-41,41,-41,31,-31,31,-31,28,-28,28,-28,8,-8,8,-8,39,-39,39,-39,15,-15,15,-15,32,-32,32,-32,27,-27,27,-27,35,-35,35,-35,23,-23,23,-23,41,-41,41,-41,9,-9,9,-9,42,-42,0,0,42,-42,42,-42,33,-33,33,-33,26,-26,26,-26,1,-1,1,-1,42,-42,42,-42,38,-38,38,-38,18,-18,18,-18,2,-2,2,-2,40,-40,40,-40,37,-37,37,-37,20,-20,20,-20,13,-13,13,-13,42,-42,42,-42,3,-3,3,-3,39,-39,39,-39,16,-16,16,-16,42,-42,42,-42,36,-36,36,-36,22,-22,22,-22,4,-4,4,-4,41,-41,41,-41,34,-34,34,-34,25,-25,25,-25,10,-10,10,-10,42,-42,42,-42,5,-5,5,-5,40,-40,40,-40,14,-14,14,-14,42,-42,42,-42,30,-30,30,-30,6,-6,6,-6,35,-35,35,-35,24,-24,24,-24,41,-41,41,-41,31,-31,31,-31,29,-29,29,-29,11,-11,11,-11,38,-38,38,-38,19,-19,19,-19,32,-32,32,-32,28,-28,28,-28,39,-39,39,-39,37,-37,37,-37,21,-21,21,-21,17,-17,17,-17,42,-42,42,-42,7,-7,7,-7,33,-33,33,-33,27,-27,27,-27,41,-41,41,-41,40,-40,40,-40,36,-36,36,-36,23,-23,23,-23,15,-15,15,-15,12,-12,12,-12,42,-42,42,-42,8,-8,8,-8,34,-34,34,-34,26,-26,26,-26,38,-38,38,-38,20,-20,20,-20,42,-42,42,-42,39,-39,39,-39,18,-18,18,-18,9,-9,9,-9,43,-43,0,0,43,-43,43,-43,41,-41,41,-41,35,-35,35,-35,25,-25,25,-25,13,-13,13,-13,1,-1,1,-1,43,-43,43,-43,37,-37,37,-37,22,-22,22,-22,2,-2,2,-2,40,-40,40,-40,16,-16,16,-16,43,-43,43,-43,3,-3,3,-3,31,-31,31,-31,30,-30,30,-30,42,-42,42,-42,10,-10,10,-10,43,-43,43,-43,32,-32,32,-32,29,-29,29,-29,4,-4,4,-4,36,-36,36,-36,24,-24,24,-24,33,-33,33,-33,28,-28,28,-28,43,-43,43,-43,5,-5,5,-5,41,-41,41,-41,14,-14,14,-14,39,-39,39,-39,19,-19,19,-19,43,-43,43,-43,42,-42,42,-42,38,-38,38,-38,34,-34,34,-34,27,-27,27,-27,21,-21,21,-21,11,-11,11,-11,6,-6,6,-6,40,-40,40,-40,17,-17,17,-17,43,-43,43,-43,37,-37,37,-37,23,-23,23,-23,7,-7,7,-7,35,-35,35,-35,26,-26,26,-26,41,-41,41,-41,15,-15,15,-15,42,-42,42,-42,12,-12,12,-12,43,-43,43,-43,8,-8,8,-8,39,-39,39,-39,36,-36,36,-36,25,-25,25,-25,20,-20,20,-20,31,-31,31,-31,40,-40,40,-40,32,-32,32,-32,30,-30,30,-30,18,-18,18,-18,38,-38,38,-38,22,-22,22,-22,43,-43,43,-43,33,-33,33,-33,29,-29,29,-29,9,-9,9,-9,42,-42,42,-42,13,-13,13,-13,44,-44,0,0,44,-44,44,-44,41,-41,41,-41,16,-16,16,-16,1,-1,1,-1,44,-44,44,-44,34,-34,34,-34,28,-28,28,-28,2,-2,2,-2,44,-44,44,-44,37,-37,37,-37,24,-24,24,-24,3,-3,3,-3,43,-43,43,-43,10,-10,10,-10,44,-44,44,-44,4,-4,4,-4,35,-35,35,-35,27,-27,27,-27,42,-42,42,-42,14,-14,14,-14,44,-44,44,-44,40,-40,40,-40,19,-19,19,-19,5,-5,5,-5,39,-39,39,-39,21,-21,21,-21,43,-43,43,-43,41,-41,41,-41,17,-17,17,-17,11,-11,11,-11,44,-44,44,-44,36,-36,36,-36,26,-26,26,-26,6,-6,6,-6,38,-38,38,-38,23,-23,23,-23,44,-44,44,-44,32,-32,32,-32,31,-31,31,-31,7,-7,7,-7,42,-42,42,-42,33,-33,33,-33,30,-30,30,-30,15,-15,15,-15,43,-43,43,-43,12,-12,12,-12,37,-37,37,-37,25,-25,25,-25,34,-34,34,-34,29,-29,29,-29,44,-44,44,-44,40,-40,40,-40,20,-20,20,-20,8,-8,8,-8,41,-41,41,-41,39,-39,39,-39,22,-22,22,-22,18,-18,18,-18,35,-35,35,-35,28,-28,28,-28,44,-44,44,-44,9,-9,9,-9,43,-43,43,-43,13,-13,13,-13,42,-42,42,-42,38,-38,38,-38,24,-24,24,-24,16,-16,16,-16,45,-45,36,-36,36,-36,27,-27,27,-27,0,0,45,-45,45,-45,1,-1,1,-1,45,-45,45,-45,2,-2,2,-2,45,-45,45,-45,3,-3,3,-3,44,-44,44,-44,10,-10,10,-10,45,-45,45,-45,40,-40,40,-40,21,-21,21,-21,4,-4,4,-4,41,-41,41,-41,19,-19,19,-19,43,-43,43,-43,37,-37,37,-37,26,-26,26,-26,14,-14,14,-14,32,-32,32,-32,45,-45,45,-45,39,-39,39,-39,33,-33,33,-33,31,-31,31,-31,23,-23,23,-23,5,-5,5,-5,42,-42,42,-42,17,-17,17,-17,34,-34,34,-34,30,-30,30,-30,44,-44,44,-44,11,-11,11,-11,45,-45,45,-45,6,-6,6,-6,35,-35,35,-35,29,-29,29,-29,38,-38,38,-38,25,-25,25,-25,45,-45,45,-45,43,-43,43,-43,15,-15,15,-15,7,-7,7,-7,44,-44,44,-44,36,-36,36,-36,28,-28,28,-28,12,-12,12,-12,41,-41,41,-41,20,-20,20,-20,40,-40,40,-40,22,-22,22,-22,42,-42,42,-42,18,-18,18,-18,45,-45,45,-45,8,-8,8,-8,39,-39,39,-39,24,-24,24,-24,37,-37,37,-37,27,-27,27,-27,44,-44,44,-44,43,-43,43,-43,16,-16,16,-16,13,-13,13,-13,45,-45,45,-45,9,-9,9,-9,33,-33,33,-33,32,-32,32,-32,46,-46,0,0,46,-46,46,-46,34,-34,34,-34,31,-31,31,-31,1,-1,1,-1,46,-46,46,-46,38,-38,38,-38,26,-26,26,-26,2,-2,2,-2,41,-41,41,-41,21,-21,21,-21,46,-46,46,-46,45,-45,45,-45,42,-42,42,-42,35,-35,35,-35,30,-30,30,-30,19,-19,19,-19,10,-10,10,-10,3,-3,3,-3,40,-40,40,-40,23,-23,23,-23,46,-46,46,-46,44,-44,44,-44,14,-14,14,-14,4,-4,4,-4,36,-36,36,-36,29,-29,29,-29,43,-43,43,-43,17,-17,17,-17,46,-46,46,-46,5,-5,5,-5,45,-45,45,-45,39,-39,39,-39,25,-25,25,-25,11,-11,11,-11,46,-46,46,-46,6,-6,6,-6,37,-37,37,-37,28,-28,28,-28,44,-44,44,-44,15,-15,15,-15,42,-42,42,-42,20,-20,20,-20,46,-46,46,-46,41,-41,41,-41,22,-22,22,-22,7,-7,7,-7,45,-45,45,-45,12,-12,12,-12,43,-43,43,-43,38,-38,38,-38,27,-27,27,-27,18,-18,18,-18,40,-40,40,-40,24,-24,24,-24,33,-33,33,-33,46,-46,46,-46,34,-34,34,-34,32,-32,32,-32,8,-8,8,-8,35,-35,35,-35,31,-31,31,-31,44,-44,44,-44,16,-16,16,-16,45,-45,45,-45,13,-13,13,-13,36,-36,36,-36,30,-30,30,-30,46,-46,46,-46,39,-39,39,-39,26,-26,26,-26,9,-9,9,-9,42,-42,42,-42,21,-21,21,-21,47,-47,0,0,47,-47,47,-47,43,-43,43,-43,41,-41,41,-41,37,-37,37,-37,29,-29,29,-29,23,-23,23,-23,19,-19,19,-19,1,-1,1,-1,47,-47,47,-47,2,-2,2,-2,46,-46,46,-46,10,-10,10,-10,47,-47,47,-47,3,-3,3,-3,45,-45,45,-45,14,-14,14,-14,47,-47,47,-47,44,-44,44,-44,40,-40,40,-40,25,-25,25,-25,17,-17,17,-17,4,-4,4,-4,38,-38,38,-38,28,-28,28,-28,47,-47,47,-47,5,-5,5,-5,46,-46,46,-46,11,-11,11,-11,47,-47,47,-47,34,-34,34,-34,33,-33,33,-33,6,-6,6,-6,42,-42,42,-42,22,-22,22,-22,43,-43,43,-43,35,-35,35,-35,32,-32,32,-32,20,-20,20,-20,45,-45,45,-45,39,-39,39,-39,27,-27,27,-27,15,-15,15,-15,41,-41,41,-41,36,-36,36,-36,31,-31,31,-31,24,-24,24,-24,47,-47,47,-47,7,-7,7,-7,46,-46,46,-46,44,-44,44,-44,18,-18,18,-18,12,-12,12,-12,37,-37,37,-37,30,-30,30,-30,47,-47,47,-47,8,-8,8,-8,40,-40,40,-40,26,-26,26,-26,45,-45,45,-45,16,-16,16,-16,46,-46,46,-46,38,-38,38,-38,29,-29,29,-29,13,-13,13,-13,47,-47,47,-47,43,-43,43,-43,21,-21,21,-21,9,-9,9,-9,42,-42,42,-42,23,-23,23,-23,44,-44,44,-44,19,-19,19,-19,48,-48,0,0,48,-48,48,-48,39,-39,39,-39,28,-28,28,-28,1,-1,1,-1,41,-41,41,-41,25,-25,25,-25,48,-48,48,-48,2,-2,2,-2,47,-47,47,-47,10,-10,10,-10,46,-46,46,-46,34,-34,34,-34,14,-14,14,-14,48,-48,48,-48,3,-3,3,-3,45,-45,45,-45,35,-35,35,-35,33,-33,33,-33,17,-17,17,-17,48,-48,48,-48,36,-36,36,-36,32,-32,32,-32,4,-4,4,-4,48,-48,48,-48,40,-40,40,-40,27,-27,27,-27,5,-5,5,-5,47,-47,47,-47,37,-37,37,-37,31,-31,31,-31,11,-11,11,-11,43,-43,43,-43,22,-22,22,-22,44,-44,44,-44,20,-20,20,-20,48,-48,48,-48,42,-42,42,-42,24,-24,24,-24,6,-6,6,-6,46,-46,46,-46,15,-15,15,-15,38,-38,38,-38,30,-30,30,-30,45,-45,45,-45,18,-18,18,-18,48,-48,48,-48,47,-47,47,-47,12,-12,12,-12,7,-7,7,-7,41,-41,41,-41,26,-26,26,-26,39,-39,39,-39,29,-29,29,-29,48,-48,48,-48,8,-8,8,-8,46,-46,46,-46,16,-16,16,-16,44,-44,44,-44,21,-21,21,-21,47,-47,47,-47,43,-43,43,-43,23,-23,23,-23,13,-13,13,-13,35,-35,35,-35,34,-34,34,-34,40,-40,40,-40,28,-28,28,-28,48,-48,48,-48,36,-36,36,-36,33,-33,33,-33,9,-9,9,-9,45,-45,45,-45,19,-19,19,-19,42,-42,42,-42,25,-25,25,-25,37,-37,37,-37,32,-32,32,-32,49,-49,0,0,49,-49,49,-49,1,-1,1,-1,48,-48,48,-48,10,-10,10,-10,49,-49,49,-49,47,-47,47,-47,46,-46,46,-46,38,-38,38,-38,31,-31,31,-31,17,-17,17,-17,14,-14,14,-14,2,-2,2,-2,49,-49,49,-49,41,-41,41,-41,27,-27,27,-27,3,-3,3,-3,49,-49,49,-49,4,-4,4,-4,44,-44,44,-44,22,-22,22,-22,39,-39,39,-39,30,-30,30,-30,48,-48,48,-48,45,-45,45,-45,43,-43,43,-43,24,-24,24,-24,20,-20,20,-20,11,-11,11,-11,49,-49,49,-49,5,-5,5,-5,47,-47,47,-47,15,-15,15,-15,49,-49,49,-49,6,-6,6,-6,46,-46,46,-46,42,-42,42,-42,26,-26,26,-26,18,-18,18,-18,40,-40,40,-40,29,-29,29,-29,48,-48,48,-48,12,-12,12,-12,49,-49,49,-49,35,-35,35,-35,7,-7,7,-7,36,-36,36,-36,34,-34,34,-34,37,-37,37,-37,33,-33,33,-33,49,-49,49,-49,47,-47,47,-47,44,-44,44,-44,41,-41,41,-41,28,-28,28,-28,23,-23,23,-23,16,-16,16,-16,8,-8,8,-8,45,-45,45,-45,21,-21,21,-21,38,-38,38,-38,32,-32,32,-32,48,-48,48,-48,13,-13,13,-13,43,-43,43,-43,25,-25,25,-25,46,-46,46,-46,19,-19,19,-19,49,-49,49,-49,39,-39,39,-39,31,-31,31,-31,9,-9,9,-9,42,-42,42,-42,27,-27,27,-27,47,-47,47,-47,17,-17,17,-17,50,-50,48,-48,48,-48,40,-40,40,-40,30,-30,30,-30,14,-14,14,-14,0,0}; diff --git a/cpp/a_star/expansion_groups.py b/cpp/a_star/expansion_groups.py new file mode 100644 index 0000000..e2a2490 --- /dev/null +++ b/cpp/a_star/expansion_groups.py @@ -0,0 +1,60 @@ +from os import getcwd +from os.path import join, dirname +from math import sqrt + +MAX_RADIUS = 5 +LINES = 321 +COLS = 221 + +expansion_groups = dict() +CWD = join(getcwd(), dirname(__file__)) + +for l in range(MAX_RADIUS*10+1): + for c in range(MAX_RADIUS*10+1): + dist = sqrt(l*l+c*c)*0.1 + if dist <= MAX_RADIUS: + if not dist in expansion_groups: + expansion_groups[dist] = [] + expansion_groups[dist].append((l,c)) + + if c>0: expansion_groups[dist].append((l,-c)) + if l>0: expansion_groups[dist].append((-l,c)) + if c>0 and l>0: expansion_groups[dist].append((-l,-c)) + +ascending_dist = sorted(expansion_groups) +groups_no = len(expansion_groups) + +#============================================= prepare text to file + +no_of_pos = 0 +for g in expansion_groups.values(): + no_of_pos += len(g) + +file = [] +file.append(f"const int expansion_positions_no = {no_of_pos};\n") +file.append(f"const float expansion_pos_dist[{no_of_pos}] = {{") + +for dist in ascending_dist: + for p in expansion_groups[dist]: + file.extend([f"{dist}",","]) +file.pop() + +file.append(f"}};\n\nconst int expansion_pos_l[{no_of_pos}] = {{") + +for dist in ascending_dist: + for p in expansion_groups[dist]: + file.extend([f"{p[0]}",","]) +file.pop() + +file.append(f"}};\n\nconst int expansion_pos_c[{no_of_pos}] = {{") + +for dist in ascending_dist: + for p in expansion_groups[dist]: + file.extend([f"{p[1]}",","]) +file.pop() +file.append("};\n") + +#============================================= write to file + +with open(join(CWD, "expansion_groups.h"), 'w') as f: + f.write(''.join(file)) diff --git a/cpp/a_star/lib_main.cpp b/cpp/a_star/lib_main.cpp new file mode 100644 index 0000000..088c0ae --- /dev/null +++ b/cpp/a_star/lib_main.cpp @@ -0,0 +1,43 @@ +#include "a_star.h" +#include +#include + +namespace py = pybind11; +using namespace std; + + +py::array_t compute( py::array_t parameters ){ + + // ================================================= 1. Parse data + + py::buffer_info parameters_buf = parameters.request(); + int params_len = parameters_buf.shape[0]; + + // ================================================= 2. Compute path + + astar( (float*)parameters_buf.ptr, params_len ); + + // ================================================= 3. Prepare data to return + + py::array_t retval = py::array_t(final_path_size); //allocate + py::buffer_info buff = retval.request(); + float *ptr = (float *) buff.ptr; + + for(int i=0; i "argname"_a + +PYBIND11_MODULE(a_star, m) { // the python module name, m is the interface to create bindings + m.doc() = "Custom A-star implementation"; // optional module docstring + + // optional arguments names + m.def("compute", &compute, "Compute the best path", "parameters"_a); +} \ No newline at end of file diff --git a/cpp/ball_predictor/Makefile b/cpp/ball_predictor/Makefile new file mode 100644 index 0000000..c60f708 --- /dev/null +++ b/cpp/ball_predictor/Makefile @@ -0,0 +1,14 @@ +src = $(wildcard *.cpp) +obj = $(src:.c=.o) + +CFLAGS = -O3 -shared -std=c++11 -fPIC -Wall $(PYBIND_INCLUDES) + +all: $(obj) + g++ $(CFLAGS) -o ball_predictor.so $^ + +debug: $(filter-out lib_main.cpp,$(obj)) + g++ -O0 -std=c++14 -Wall -g -o debug.bin debug_main.cc $^ + +.PHONY: clean +clean: + rm -f $(obj) all diff --git a/cpp/ball_predictor/ball_predictor.cpp b/cpp/ball_predictor/ball_predictor.cpp new file mode 100644 index 0000000..7cf769a --- /dev/null +++ b/cpp/ball_predictor/ball_predictor.cpp @@ -0,0 +1,104 @@ +#include +#include "ball_predictor.h" + + +float ball_pos_pred[600]; // ball position (x,y) prediction for 300*0.02s = 6s +float ball_vel_pred[600]; // ball velocity (x,y) prediction for 300*0.02s = 6s +float ball_spd_pred[300]; // ball linear speed (s) prediction for 300*0.02s = 6s +int pos_pred_len=0; + +/** + * @brief Get intersection with moving ball (intersection point and distance) + * @param x robot position (x) + * @param y robot position (y) + * @param max_robot_sp_per_step maximum speed per step + * @param ball_pos imported ball positions (possibly modified version of ball_pos_pred) + * @param ball_pos_len length of ball_pos + * @param ret_x returned position (x) of intersection point + * @param ret_y returned position (y) of intersection point + * @param ret_d returned distance between robot and intersection point + */ +void get_intersection_with_ball(float x, float y, float max_robot_sp_per_step, float ball_pos[], float ball_pos_len, + float &ret_x, float &ret_y, float &ret_d){ + + float robot_max_displacement = 0.2; // robot has an immediate reach radius of 0.2m + int j=0; + + while(1){ + float vec_x = ball_pos[j++] - x; + float vec_y = ball_pos[j++] - y; + float b_dist_sq = vec_x*vec_x + vec_y*vec_y; // squared ball distance + + // If robot has reached the ball, or the ball has stopped but the robot is still not there + if (b_dist_sq <= robot_max_displacement*robot_max_displacement or j>=ball_pos_len){ + float d = sqrtf(b_dist_sq); + ret_d = d; + ret_x = ball_pos[j-2]; + ret_y = ball_pos[j-1]; + break; + } + robot_max_displacement += max_robot_sp_per_step; + } +} + + +/** + * @brief Predict ball position/velocity until the ball stops or gets out of bounds (up to 6s) + * Adequate when the ball is rolling on the ground + * @param bx ball position (x) + * @param by ball position (y) + * @param vx ball velocity (x) + * @param vy ball velocity (y) + */ +void predict_rolling_ball_pos_vel_spd(double bx, double by, double vx, double vy){ + + // acceleration = Rolling Drag Force * mass (constant = 0.026 kg) + // acceleration = k1 * velocity^2 + k2 * velocity + const double k1 = -0.01; + const double k2 = -1; + + const double k1_x = (vx < 0) ? -k1 : k1; // invert k1 if vx is negative, because vx^2 absorbs the sign + const double k1_y = (vy < 0) ? -k1 : k1; // invert k1 if vy is negative, because vy^2 absorbs the sign + + ball_pos_pred[0] = bx; // current ball position + ball_pos_pred[1] = by; + ball_vel_pred[0] = vx; // current ball velocity + ball_vel_pred[1] = vy; + ball_spd_pred[0] = sqrt(vx*vx+vy*vy); + + int counter = 2; + + while(counter < 600){ + + // acceleration + double acc_x = vx*vx*k1_x + vx*k2; + double acc_y = vy*vy*k1_y + vy*k2; + + // second equation of motion: displacement = v0*t + 0.5*a*t^2 + double dx = vx*0.02 + acc_x*0.0002; // 0.5*0.02^2 = 0.0002 + double dy = vy*0.02 + acc_y*0.0002; // 0.5*0.02^2 = 0.0002 + + // position + bx += dx; + by += dy; + + // abort when displacement is low or ball is out of bounds + if ((fabs(dx) < 0.0005 and fabs(dy) < 0.0005) or fabs(bx) > 15 or fabs(by) > 10){ + break; + } + + // velocity + vx += acc_x*0.02; + vy += acc_y*0.02; + + // store as 32b + ball_spd_pred[counter/2] = sqrt(vx*vx+vy*vy); + ball_vel_pred[counter] = vx; + ball_pos_pred[counter++] = bx; + ball_vel_pred[counter] = vy; + ball_pos_pred[counter++] = by; + + } + + pos_pred_len = counter; +} \ No newline at end of file diff --git a/cpp/ball_predictor/ball_predictor.h b/cpp/ball_predictor/ball_predictor.h new file mode 100644 index 0000000..22018b4 --- /dev/null +++ b/cpp/ball_predictor/ball_predictor.h @@ -0,0 +1,10 @@ +#pragma once + +extern float ball_pos_pred[600]; // ball position (x,y) prediction for 300*0.02s = 6s +extern float ball_vel_pred[600]; // ball velocoty (x,y) prediction for 300*0.02s = 6s +extern float ball_spd_pred[300]; // ball linear speed (s) prediction for 300*0.02s = 6s +extern int pos_pred_len; + +extern void get_intersection_with_ball(float x, float y, float max_robot_sp_per_step, float ball_pos[], float ball_pos_len, + float &ret_x, float &ret_y, float &ret_d); +extern void predict_rolling_ball_pos_vel_spd(double bx, double by, double vx, double vy); diff --git a/cpp/ball_predictor/debug_main.cc b/cpp/ball_predictor/debug_main.cc new file mode 100644 index 0000000..469c4f3 --- /dev/null +++ b/cpp/ball_predictor/debug_main.cc @@ -0,0 +1,54 @@ +#include "ball_predictor.h" +#include +#include +#include + +using std::cout; +using std::chrono::high_resolution_clock; +using std::chrono::duration_cast; +using std::chrono::microseconds; + +std::chrono::_V2::system_clock::time_point t1,t2; + +int main(){ + + // ================================================= 1. Generate data + + float px = 3; + float py = 4; + float vx = -5; + float vy = -1; + + // ================================================= 2. Compute prediction + + t1 = high_resolution_clock::now(); + predict_rolling_ball_pos_vel_spd(px, py, vx, vy); + t2 = high_resolution_clock::now(); + + cout << std::fixed << std::setprecision(8); + + for(int i=0; i(t2 - t1).count() << "us for prediction\n"; + + // ================================================= 3. Generate data + + float robot_x = -1; + float robot_y = 1; + float max_speed_per_step = 0.7*0.02; + float ret_x, ret_y, ret_d; + + // ================================================= 4. Compute intersection + + t1 = high_resolution_clock::now(); + get_intersection_with_ball(robot_x, robot_y, max_speed_per_step, ball_pos_pred, pos_pred_len, ret_x, ret_y, ret_d); + t2 = high_resolution_clock::now(); + + cout << duration_cast(t2 - t1).count() << "us for intersection\n\n"; + cout << "Intersection: " << ret_x << "," << ret_y << " dist: " << ret_d << "\n\n"; + +} diff --git a/cpp/ball_predictor/lib_main.cpp b/cpp/ball_predictor/lib_main.cpp new file mode 100644 index 0000000..9d6be32 --- /dev/null +++ b/cpp/ball_predictor/lib_main.cpp @@ -0,0 +1,100 @@ +#include "ball_predictor.h" +#include +#include + +namespace py = pybind11; +using namespace std; + + +/** + * @brief Predict rolling ball position, velocity, linear speed + * + * @param parameters + * ball_x, ball_y, ball_vel_x, ball_vel_y + * @return ball_pos_pred, ball_vel_pred, ball_spd_pred + */ +py::array_t predict_rolling_ball( py::array_t parameters ){ + + // ================================================= 1. Parse data + + py::buffer_info parameters_buf = parameters.request(); + float* parameters_ptr = (float*)parameters_buf.ptr; + + float px = parameters_ptr[0]; + float py = parameters_ptr[1]; + float vx = parameters_ptr[2]; + float vy = parameters_ptr[3]; + + // ================================================= 2. Compute path + + predict_rolling_ball_pos_vel_spd(px, py, vx, vy); + + // ================================================= 3. Prepare data to return + + py::array_t retval = py::array_t(pos_pred_len+pos_pred_len+pos_pred_len/2); //allocate + py::buffer_info buff = retval.request(); + float *ptr = (float *) buff.ptr; + + for(int i=0; i get_intersection( py::array_t parameters ){ + + // ================================================= 1. Parse data + + py::buffer_info parameters_buf = parameters.request(); + float* parameters_ptr = (float*)parameters_buf.ptr; + int params_len = parameters_buf.shape[0]; + + float x = parameters_ptr[0]; + float y = parameters_ptr[1]; + float max_sp = parameters_ptr[2]; + float* ball_pos = parameters_ptr + 3; + float ret_x, ret_y, ret_d; + + // ================================================= 2. Compute path + + get_intersection_with_ball(x, y, max_sp, ball_pos, params_len-3, ret_x, ret_y, ret_d); + + // ================================================= 3. Prepare data to return + + py::array_t retval = py::array_t(3); //allocate + py::buffer_info buff = retval.request(); + float *ptr = (float *) buff.ptr; + + ptr[0] = ret_x; + ptr[1] = ret_y; + ptr[2] = ret_d; + return retval; +} + + +using namespace pybind11::literals; // to add informative argument names as -> "argname"_a + +PYBIND11_MODULE(ball_predictor, m) { // the python module name, m is the interface to create bindings + m.doc() = "Ball predictor"; // optional module docstring + + // optional arguments names + m.def("predict_rolling_ball", &predict_rolling_ball, "Predict rolling ball", "parameters"_a); + m.def("get_intersection", &get_intersection, "Get point of intersection with moving ball", "parameters"_a); +} + diff --git a/cpp/localization/Field.cpp b/cpp/localization/Field.cpp new file mode 100644 index 0000000..f158d6b --- /dev/null +++ b/cpp/localization/Field.cpp @@ -0,0 +1,532 @@ +#include "Field.h" +#include "RobovizLogger.h" +#include "World.h" + + +static World& world = SWorld::getInstance(); + +//================================================================================================= +//=========================================================================== constexpr definitions +//================================================================================================= + +decltype(Field::cRingLineLength) constexpr Field::cRingLineLength; +decltype(Field::cPenaltyBoxDistX) constexpr Field::cPenaltyBoxDistX; +decltype(Field::cHalfPenaltyWidth) constexpr Field::cHalfPenaltyWidth; +decltype(Field::cHalfGoalWidth) constexpr Field::cHalfGoalWidth; +decltype(Field::cHalfFielfLength) constexpr Field::cHalfFielfLength; +decltype(Field::cGoalWidth) constexpr Field::cGoalWidth; +decltype(Field::cGoalDepth) constexpr Field::cGoalDepth; +decltype(Field::cGoalHeight) constexpr Field::cGoalHeight; +decltype(Field::cFieldLength) constexpr Field::cFieldLength; +decltype(Field::cFieldWidth) constexpr Field::cFieldWidth; +decltype(Field::cPenaltyLength) constexpr Field::cPenaltyLength; +decltype(Field::cPenaltyWidth) constexpr Field::cPenaltyWidth; +decltype(Field::cFieldLineSegments::list) constexpr Field::cFieldLineSegments::list; +decltype(Field::cFieldPoints::list) constexpr Field::cFieldPoints::list; + +//non-constexpr definitions +decltype(Field::list_8_landmarks::list) Field::list_8_landmarks::list; + +//================================================================================================= +//=============================================================================== Drawing utilities +//================================================================================================= + + +/** + * Draw estimates of all visible lines, markers, self position and ball + * */ + +void Field::draw_visible(const Matrix4D& headToFieldT, bool is_right_side) const{ + + if(is_right_side){ + return draw_visible_switch(headToFieldT); + } + + string draw_name = "localization"; + + RobovizLogger* roboviz = RobovizLogger::Instance(); + roboviz->init(); // only initialized when draw_visible is called (only happens once) + + //--------------------------------- Print all lines, whether they were identified or not + for(const Line6f& l : list_segments) { + + Vector3f s = headToFieldT * l.startc; + Vector3f e = headToFieldT * l.endc; + + roboviz->drawLine(s.x, s.y, s.z, e.x, e.y, e.z, 1, 0.8,0,0, &draw_name); + } + + //--------------------------------- Print identified line segments, with their fixed abs coordinates + + for(const auto& s : list_known_segments){ + + Vector3f mid = Vector3f::determineMidpoint(s.point[0].absPos.get_vector(), s.point[1].absPos.get_vector()); + + string line_name(s.fieldSegment->name); + roboviz->drawAnnotation(&line_name,mid.x,mid.y,mid.z, 0,1,0,&draw_name); + roboviz->drawLine(s.point[0].absPos.x, s.point[0].absPos.y, s.point[0].absPos.z, + s.point[1].absPos.x, s.point[1].absPos.y, s.point[1].absPos.z, 3, 0,0.8,0, &draw_name); + } + + for(const auto& m : list_known_markers){ + string line_name(m.fieldPt->name); + roboviz->drawAnnotation(&line_name,m.absPos.x, m.absPos.y, m.absPos.z+1, 1,0,0,&draw_name); + roboviz->drawLine(m.absPos.x, m.absPos.y, m.absPos.z, + m.absPos.x, m.absPos.y, m.absPos.z+0.5, 1, 0.8,0.8,0.8, &draw_name); + } + for(const auto& m : list_unknown_markers){ + string line_name = "!"; + roboviz->drawAnnotation(&line_name,m.absPos.x, m.absPos.y, m.absPos.z+1, 1,0,0,&draw_name); + roboviz->drawLine(m.absPos.x, m.absPos.y, m.absPos.z, + m.absPos.x, m.absPos.y, m.absPos.z+0.5, 1, 0.8,0.8,0.8, &draw_name); + } + + //--------------------------------- Draw player and ball arrows + + Vector3f me = headToFieldT.toVector3f(); + + roboviz->drawLine(me.x, me.y, me.z, me.x, me.y, me.z+0.5, 2,1,0,0,&draw_name); + roboviz->drawLine(me.x, me.y, me.z, me.x-0.2, me.y, me.z+0.2, 2,1,0,0,&draw_name); + roboviz->drawLine(me.x, me.y, me.z, me.x+0.2, me.y, me.z+0.2, 2,1,0,0,&draw_name); + + //There is no need to draw the ball position here (but it works well) + + /*static Vector3f last_known_ball_pos = Vector3f(); + if(world.ball_seen){ + last_known_ball_pos = headToFieldT * world.ball_rel_pos_cart; + } + + Vector3f &b = last_known_ball_pos; + + roboviz->drawLine(b.x, b.y, b.z, b.x, b.y, b.z+0.5, 2,1,1,0,&draw_name); + roboviz->drawLine(b.x, b.y, b.z, b.x-0.2, b.y, b.z+0.2, 2,1,1,0,&draw_name); + roboviz->drawLine(b.x, b.y, b.z, b.x+0.2, b.y, b.z+0.2, 2,1,1,0,&draw_name);*/ + + roboviz->swapBuffers(&draw_name); + + +} + +/** + * Draw estimates of all visible lines, markers, self position and ball, but switch field sides + * */ + +void Field::draw_visible_switch(const Matrix4D& headToFieldT) const{ + + string draw_name = "localization"; + + RobovizLogger* roboviz = RobovizLogger::Instance(); + roboviz->init(); // only initialized when draw_visible is called (only happens once) + + //--------------------------------- Print all lines, whether they were identified or not + for(const Line6f& l : list_segments) { + + Vector3f s = headToFieldT * l.startc; + Vector3f e = headToFieldT * l.endc; + + roboviz->drawLine(-s.x, -s.y, s.z, -e.x, -e.y, e.z, 1, 0.8,0,0, &draw_name); + } + + //--------------------------------- Print identified line segments, with their fixed abs coordinates + + for(const auto& s : list_known_segments){ + + Vector3f mid = Vector3f::determineMidpoint(s.point[0].absPos.get_vector(), s.point[1].absPos.get_vector()); + + string line_name(s.fieldSegment->name); + roboviz->drawAnnotation(&line_name,-mid.x,-mid.y,mid.z, 0,1,0,&draw_name); + roboviz->drawLine(-s.point[0].absPos.x, -s.point[0].absPos.y, s.point[0].absPos.z, + -s.point[1].absPos.x, -s.point[1].absPos.y, s.point[1].absPos.z, 3, 0,0.8,0, &draw_name); + } + + for(const auto& m : list_known_markers){ + string line_name(m.fieldPt->name); + roboviz->drawAnnotation(&line_name,-m.absPos.x, -m.absPos.y, m.absPos.z+1, 1,0,0,&draw_name); + roboviz->drawLine(-m.absPos.x, -m.absPos.y, m.absPos.z, + -m.absPos.x, -m.absPos.y, m.absPos.z+0.5, 1, 0.8,0.8,0.8, &draw_name); + } + for(const auto& m : list_unknown_markers){ + string line_name = "!"; + roboviz->drawAnnotation(&line_name,-m.absPos.x, -m.absPos.y, m.absPos.z+1, 1,0,0,&draw_name); + roboviz->drawLine(-m.absPos.x, -m.absPos.y, m.absPos.z, + -m.absPos.x, -m.absPos.y, m.absPos.z+0.5, 1, 0.8,0.8,0.8, &draw_name); + } + + //--------------------------------- Draw player and ball arrows + + Vector3f me = headToFieldT.toVector3f(); + + roboviz->drawLine(-me.x, -me.y, me.z, -me.x, -me.y, me.z+0.5, 2,1,0,0,&draw_name); + roboviz->drawLine(-me.x, -me.y, me.z, -me.x+0.2, -me.y, me.z+0.2, 2,1,0,0,&draw_name); + roboviz->drawLine(-me.x, -me.y, me.z, -me.x-0.2, -me.y, me.z+0.2, 2,1,0,0,&draw_name); + + //There is no need to draw the ball position here (but it works well) + + /*static Vector3f last_known_ball_pos = Vector3f(); + if(world.ball_seen){ + last_known_ball_pos = headToFieldT * world.ball_rel_pos_cart; + } + + Vector3f &b = last_known_ball_pos; + + roboviz->drawLine(b.x, b.y, b.z, b.x, b.y, b.z+0.5, 2,1,1,0,&draw_name); + roboviz->drawLine(b.x, b.y, b.z, b.x-0.2, b.y, b.z+0.2, 2,1,1,0,&draw_name); + roboviz->drawLine(b.x, b.y, b.z, b.x+0.2, b.y, b.z+0.2, 2,1,1,0,&draw_name);*/ + + roboviz->swapBuffers(&draw_name); + + +} + + +//================================================================================================= +//==================================================================== Refresh / Identify / Collect +//================================================================================================= + + +/** + * Gather markers with known absolute z: line endpoints + foot contact points + [toe contact points] + **/ +void Field::gather_ground_markers(){ + + /** + * Add NAO's feet ground contact points to zmarks + * Dependency: the agent's feet must be touching the ground + * Flaws: + * - if the feet are touching other players or the ball (may be solved if it is problematic) + * - robot 4 may be touching the ground with its toes and they are currently ignored + **/ + + for(int i=0; i<2; i++){ + if( world.foot_touch[i] ){ //if this foot is touching the ground + + //Vector3f contactAux = world.foot_contact_pt[i]; //contact point using strange coordinate system + //Vector3f contactpt = Vector3f(contactAux.y,-contactAux.x,contactAux.z); // fix coordinate system for both feet + + Vector3f relPos = world.foot_contact_rel_pos[i]; + list_feet_contact_points.emplace_back( sVector3d({0,0,0}), relPos.toPolar(), relPos); + list_ground_markers.emplace_back( sVector3d({0,0,0}), relPos.toPolar(), relPos); + } + } + + //Deactivated since it did not produce better results, even when both feet are floating (it was only better when the robot fell) + /*const Types::BodyParts toePart[2] = {Types::ilLToe, Types::ilRToe}; + for(int i=0; i<2; i++){ + if( agent::model->getToeTouch(feetSide[i]) ){ //if this foot is touching the ground + + Vector3f contactAux = agent::model->getToeContact(feetSide[i]); //contact point using strange coordinate system + Vector3f contactpt = Vector3f(contactAux.y,-contactAux.x,contactAux.z); // fix coordinate system for both feet + Vector3f relPos = agent::model->getRelPositionOfBodyPoint(agent::model->getBodyPart(toePart[i]),contactpt); + + if(agent::model->getFootTouch(Types::iLeft) == false && agent::model->getFootTouch(Types::iRight) == false){ + zmarks.emplace_back( 0,0,0,relPos ); + } + } + }*/ + + + //Add all line endings to ground markers + for(const Line6f& l : list_segments){ + list_ground_markers.emplace_back( sVector3d({0,0,0}), l.startp, l.startc); + list_ground_markers.emplace_back( sVector3d({0,0,0}), l.endp, l.endc); + } + + non_collinear_ground_markers = list_ground_markers.size(); //Excludes corner flags + + //Add corner flags + for(const auto& c : list_landmarks_corners){ + list_ground_markers.emplace_back( sVector3d({0,0,0}), c.relPosPolar, c.relPosCart); + } + + + /** + * All the polar coordinates' errors are dependent on the distance + * Var[distance error] = Var[ed*d/100] + Var[er] (ed-error distance, er-error rounding) + * Var[distance error] = (d/100)^2 * Var[ed] + Var[er] + * Their importance will be given by Inverse-variance weighting + * + * Application: + * repetition = max(int(k*(1/var)),1), where k=1/1500 + * repetitions for 1 meter: 71 + * repetitions for 2 meters: 55 + * repetitions for >=19 meters: 1 + */ + + for(const auto& g : list_ground_markers){ + float var = pow(g.relPosPolar.x / 100.f,2) * var_distance + var_round_hundredth; + float w = 1.f/(1500.f*var); //weight = (1/var)*k where k is a constant to transform the large weights into number of repetitions + int repetitions = max(int(w),1); + + list_weighted_ground_markers.insert(list_weighted_ground_markers.end(), repetitions, g); + } +} + + +/** + * Update markers after visual step + * Marks attributes: abs(x,y,z), rel(r,h,v) + * Possible markers: + * - 8 landmarks + * - line endings: + * - 8 @ field corners + * - 12 @ penalty box corners + * - 20 @ center ring + * - 2 @ halfway line + * - 8 noisy estimates from corner-originated lines + * */ +void Field::update(){ + + //no need to reserve space since these vectors will expand mostly in the first cycles + list_segments.clear(); + list_landmarks.clear(); + list_landmarks_corners.clear(); + list_landmarks_goalposts.clear(); + list_feet_contact_points.clear(); + list_known_markers.clear(); + list_unknown_markers.clear(); + list_known_segments.clear(); + list_ground_markers.clear(); + list_weighted_ground_markers.clear(); + + //----------------------------------------- Pre-processing: prepare landmark lists + + for(int i=0; i<8; i++){ + sFixedMarker *l8; + const sFieldPoint *fp; + World::sLMark *l = &world.landmark[i]; + if (l->pos.x == -15 && l->pos.y == -10) {l8 = &list_8_landmarks::_corner_mm; fp = &cFieldPoints::corner_mm;} + else if(l->pos.x == -15 && l->pos.y == +10) {l8 = &list_8_landmarks::_corner_mp; fp = &cFieldPoints::corner_mp;} + else if(l->pos.x == +15 && l->pos.y == -10) {l8 = &list_8_landmarks::_corner_pm; fp = &cFieldPoints::corner_pm;} + else if(l->pos.x == +15 && l->pos.y == +10) {l8 = &list_8_landmarks::_corner_pp; fp = &cFieldPoints::corner_pp;} + else if(l->pos.x == -15 && l->pos.y < 0) {l8 = &list_8_landmarks::_goal_mm; fp = &cFieldPoints::goal_mm; } + else if(l->pos.x == -15 && l->pos.y > 0) {l8 = &list_8_landmarks::_goal_mp; fp = &cFieldPoints::goal_mp; } + else if(l->pos.x == +15 && l->pos.y < 0) {l8 = &list_8_landmarks::_goal_pm; fp = &cFieldPoints::goal_pm; } + else if(l->pos.x == +15 && l->pos.y > 0) {l8 = &list_8_landmarks::_goal_pp; fp = &cFieldPoints::goal_pp; } + else{ return; } + + if(l->seen){ + l8->set_relPos(l->rel_pos); + l8->visible = true; + + sMarker seen_mark(fp, l->rel_pos); + list_landmarks.push_back(seen_mark); + list_known_markers.push_back(seen_mark); + + if (l->isCorner){ list_landmarks_corners.push_back( seen_mark); } + else { list_landmarks_goalposts.push_back(seen_mark); } + }else{ + l8->visible = false; + } + } + + //----------------------------------------- Pre-processing: prepare lines and landmarks' coordinates sign + + for(const auto& l : world.lines_polar) { + list_segments.emplace_back(l.start, l.end); + } + + //----------------------------------------- Gather markers with known absolute z: line endpoints + foot contact points + + gather_ground_markers(); + +} + + + + + + + + +void Field::update_from_transformation(const Matrix4D& tmatrix){ + + /** + * Identify segments based on transformation matrix + * + * Identification starts from longest to shortest line + * The visible line segment is identified if there is only 1 close field line + * If there is more than 1 close field line and all but 1 were already taken, it is still identified + */ + + //----------------------------------------------- get lines ordered from largest to shortest + vector lines_descending_length; + for(auto& l : list_segments){ + lines_descending_length.push_back(&l); + } + + //Sort from largest to smallest radius + sort(lines_descending_length.begin(),lines_descending_length.end(), + [](const Line6f* a, const Line6f* b) { return (a->length > b->length); }); + + //----------------------------------------------- identify lines + + for(const Line6f* l : lines_descending_length){ + Vector3f l_abs[2] = {tmatrix * l->startc, tmatrix * l->endc}; + + float l_angle = atan2f(l_abs[1].y - l_abs[0].y, l_abs[1].x - l_abs[0].x); + + const float min_err = 0.3; //maximum allowed distance (startdist + enddist < 0.3m) + const sFieldSegment* best_line = nullptr; + for(const auto& s : cFieldLineSegments::list){ //find distance to closest field line + + //Skip field line if seen line is substantially larger + if( l->length > (s.length + 0.7) ){ continue; } + + //Skip field line if orientation does not match + float line_angle_difference = normalize_line_angle_rad(l_angle - s.angle); + if(line_angle_difference > 0.26) continue; //tolerance 15deg + + //Skip field line if it was already identified + bool already_identified = false; + for(const auto& k : list_known_segments){ + if(k.fieldSegment == &s){ already_identified=true; break; } + } + if(already_identified) continue; + + //Error is the sum of the distance of a single line segment to both endpoints of seen line + float err = fieldLineSegmentDistToCart2DPoint(s,l_abs[0].to2d()); + if(err < min_err) err += fieldLineSegmentDistToCart2DPoint(s,l_abs[1].to2d()); + + if(err < min_err){ + if(best_line == nullptr){ best_line = &s; } //Save the field line for now (others may emerge) + else{ + best_line = nullptr; //Two close field lines, none of which was taken yet, so abort + break; + } + } + } + + if(best_line != nullptr){ + + //-------------- Fix the seen line's start<->end order to match the corresponding field line + + int l_index[2] = {0,1}; //line index of [0]start and [1]end points + + if(normalize_vector_angle_rad(l_angle - best_line->angle) > 1.57079633f){ // they point in opposite directions + l_index[0] = 1; + l_index[1] = 0; + } + + const Vector3f *lAbs[2] = {&l_abs[l_index[0]], &l_abs[l_index[1]]}; + const Vector3f *lRelP[2] = {&l->get_polar_pt(l_index[0]), &l->get_polar_pt(l_index[1])}; + const Vector3f *lRelC[2] = {&l->get_cart_pt(l_index[0]), &l->get_cart_pt(l_index[1])}; + + //-------------- Fix the absolute coordinates with field information + + bool isInFoV[2] = {false,false}; + + //1st: recognize endpoints as field points (& save known markers) + + /** + * //---------------------------------------------- General solution + * All points reasonably within the FoV are identified + * Noise applied horizontally sigma=0.1225, Pr[-0.5 0.2 + * + * Warning 2: sometimes the error of phi and theta is larger than expected, producing points like + * (rel polar: 0.57,-36.36,-54.41) (rel cart: 0.267,-0.1967,-0.4635) + * which goes with the theory that the FoV is actually defined as a cone (a bit unrealistic though) + * current solution: cone_angle < hor_FoV-5 + */ + + for( int i=0; i<2; i++){ + float cone_angle = acosf(lRelC[i]->x / lRelP[i]->x); //angle between vector and (1,0,0) + const float max_cone_angle = (cHalfHorizontalFoV-5)*M_PI/180; + + if(cone_angle < max_cone_angle && lRelC[i]->x > 0.2){ + list_known_markers.emplace_back(best_line->point[i], *lRelP[i], *lRelC[i]); + isInFoV[i] = true; + } + } + + //2nd: use real coordinates if point was recognized, otherwise push it to a valid position (& save segment and unknown markers) + + const Line6f field_line(best_line->point[0]->get_vector(), best_line->point[1]->get_vector(), best_line->length); + + sVector3d l_pt_d[2]; //final line segment points (double precision floating-points) + + for( int i=0; i<2; i++){ + if(isInFoV[i]){ + l_pt_d[i] = best_line->point[i]->pt; //set recognized point's abs coordinates + }else{ + Vector3f p = field_line.segmentPointClosestToCartPoint(*lAbs[i]); //push point to closest valid position + l_pt_d[i].set(p); //set unknown point's estimated coordinates + list_unknown_markers.emplace_back(best_line, l_pt_d[i], *lRelP[i], *lRelC[i]); + } + } + + //-------------- Save identified line segment + list_known_segments.emplace_back(sMarker(l_pt_d[0],*lRelP[0],*lRelC[0]),sMarker(l_pt_d[1],*lRelP[1],*lRelC[1]), l->length, best_line); + + } + } +} + +void Field::update_unknown_markers(const Matrix4D& tmatrix){ + + for(auto& u : list_unknown_markers){ + + //Transform marker to world frame + Vector3f raw_abs_pos = tmatrix * u.relPosCart; + + //Push marker to existing field segment + const Line6f field_seg( u.fieldSeg->point[0]->get_vector(), u.fieldSeg->point[1]->get_vector(), u.fieldSeg->length); + Vector3f fixed_abs_pos = field_seg.segmentPointClosestToCartPoint(raw_abs_pos); //push point to closest valid position + + u.absPos = sVector3d({fixed_abs_pos.x, fixed_abs_pos.y, fixed_abs_pos.z}); + } +} + + +//================================================================================================= +//================================================================================== Math utilities +//================================================================================================= + + +/** + * Field lines are on the ground (z=0), so the method is simplified + */ +float Field::fieldLineSegmentDistToCartPoint(const sFieldSegment& fLine, const Vector3f& cp){ + + //Line segment vector (start -> end) + float vx = fLine.point[1]->pt.x - fLine.point[0]->pt.x; + float vy = fLine.point[1]->pt.y - fLine.point[0]->pt.y; + + Vector3f w1(cp.x - fLine.point[0]->pt.x, cp.y - fLine.point[0]->pt.y, cp.z); // vector: (segment start -> point) + if (w1.x * vx + w1.y * vy <= 0) + return w1.length();// if angle between vectors is >=90deg, we return the distance to segment start + + Vector3f w2(cp.x - fLine.point[1]->pt.x, cp.y - fLine.point[1]->pt.y, cp.z); // vector: (segment end -> point) + if (w2.x * vx + w2.y * vy >= 0) + return w2.length(); //if angle between vectors is <=90deg, we return the distance to segment end + + Vector3f v_cross_w1(vy * w1.z, - vx * w1.z, vx * w1.y - vy * w1.x); + return v_cross_w1.length() / fLine.length; //distance line to point (area of parallelogram divided by base gives height) + +} + +/** + * Field lines are on the ground (z=0), so the method is simplified + */ +float Field::fieldLineSegmentDistToCart2DPoint(const sFieldSegment& fLine, const Vector& cp){ + + const Vector segment_start(fLine.point[0]->pt.x, fLine.point[0]->pt.y); + const Vector segment_end( fLine.point[1]->pt.x, fLine.point[1]->pt.y ); + + //Line segment vector (start -> end) + Vector v(segment_end-segment_start); + + Vector w1(cp - segment_start);// vector: (segment start -> point) + + if(w1.innerProduct(v) <= 0) + return w1.length();// if angle between vectors is >=90deg, we return the distance to segment start + + Vector w2(cp - segment_end); // vector: (segment end -> point) + if(w2.innerProduct(v) >= 0) + return w2.length(); //if angle between vectors is <=90deg, we return the distance to segment end + + return fabsf(v.crossProduct(w1)) / fLine.length; //distance line to point (area of parallelogram divided by base gives height) +} \ No newline at end of file diff --git a/cpp/localization/Field.h b/cpp/localization/Field.h new file mode 100644 index 0000000..9d75f8b --- /dev/null +++ b/cpp/localization/Field.h @@ -0,0 +1,503 @@ +/** + * FILENAME: Field + * DESCRIPTION: Field map + * AUTHOR: Miguel Abreu (m.abreu@fe.up.pt) + * DATE: 2021 + */ + +#pragma once +#include "Vector3f.h" +#include "Singleton.h" +#include "Matrix4D.h" +#include "Line6f.h" +#include +#include + +using namespace std; + + +class Field { + friend class Singleton; + +private: + + Field(){}; + void gather_ground_markers(); + +public: + +//================================================================================================= +//====================================================================================== Structures +//================================================================================================= + + struct sVector3d { + double x,y,z; + + //sVector3d(const Vector3f& v) : x(v.x), y(v.y), z(v.z) {} + + Vector3f get_vector() const { + return Vector3f(x,y,z); + } + + void set(const sVector3d &pt){ + x=pt.x; y=pt.y; z=pt.z; + } + + void set(const Vector3f &pt){ + x=pt.x; y=pt.y; z=pt.z; + } + + float dist(const Vector3f &other) const{ + float dx = x-other.x; + float dy = y-other.y; + float dz = z-other.z; + return sqrtf(dx*dx+dy*dy+dz*dz); + } + + }; + + struct sFieldPoint { + const sVector3d pt; + const char name[10]; + + Vector3f get_vector() const { + return Vector3f(pt.x,pt.y,pt.z); + } + }; + + struct sFieldSegment { + const sFieldPoint * const point[2]; + const double length; + const double angle; + const char name[8]; + }; + + + struct sMarker { + /** + * Estimated absolute position based on the transformation matrix and field knowledge + */ + sVector3d absPos; + + /** + * Pointer to corresponding field point (if reasonably inside the FoV) + * The coordinates are the same as "absPos" but it provides other features: + * - Name of the field point + * - Knowledge that this marker corresponds to a field point (nullptr otherwise) + * - The address of the fieldPt may be compared with field segment endpoints + */ + const sFieldPoint *fieldPt = nullptr; + + /** + * Pointer to corresponding field segment + * This variable is currently set only for unknown markers + * (i.e. those which are known to belong to a field line, but whose field point is unknown) + */ + const sFieldSegment *fieldSeg = nullptr; + + Vector3f relPosPolar; + Vector3f relPosCart; + + /** + * Default constructor + */ + sMarker() : absPos({0,0,0}), relPosPolar(Vector3f()), relPosCart(Vector3f()) {}; + + /** + * Constructor with absolute position and relative polar coordinates (the cartesian version is computed) + */ + sMarker(const sVector3d& absPos_, const Vector3f& relPosPolar_) + : absPos(absPos_), relPosPolar(relPosPolar_), relPosCart(relPosPolar_.toCartesian()) {}; + + /** + * Constructor with field point and relative polar coordinates (the cartesian version is computed) + */ + sMarker(const sFieldPoint* fieldPt_, const Vector3f& relPosPolar_) + : absPos(fieldPt_->pt), fieldPt(fieldPt_), relPosPolar(relPosPolar_), relPosCart(relPosPolar_.toCartesian()) {}; + + /** + * Constructor with float absolute position and relative polar coordinates (the cartesian version is computed) + */ + sMarker(const Vector3f& absPos_, const Vector3f& relPosPolar_) + : absPos(sVector3d({absPos_.x,absPos_.y,absPos_.z})), relPosPolar(relPosPolar_), relPosCart(relPosPolar_.toCartesian()) {}; + + /** + * Constructor with absolute position, relative polar & cartesian coordinates + */ + sMarker(const sVector3d& absPos_, const Vector3f& relPosPolar_, const Vector3f& relPosCart_) + : absPos(absPos_), relPosPolar(relPosPolar_), relPosCart(relPosCart_) {}; + + /** + * Constructor with field segment, absolute position, relative polar & cartesian coordinates (e.g. unknown marker) + */ + sMarker(const sFieldSegment* fieldSeg_, const sVector3d& absPos_, const Vector3f& relPosPolar_, const Vector3f& relPosCart_) + : fieldSeg(fieldSeg_), absPos(absPos_), relPosPolar(relPosPolar_), relPosCart(relPosCart_) {}; + + /** + * Constructor with field point, relative polar & cartesian coordinates + */ + sMarker(const sFieldPoint* fieldPt_, const Vector3f& relPosPolar_, const Vector3f& relPosCart_) + : absPos(fieldPt_->pt), fieldPt(fieldPt_), relPosPolar(relPosPolar_), relPosCart(relPosCart_) {}; + + + }; + + struct sSegment { + + /** + * Order of start and end is the same as the corresponding fieldSegment + * [0]-start, [1]-end + */ + sMarker point[2]; + + float length; //visible segment length + const sFieldSegment* fieldSegment; //Corresponding field segment if we had full visibility + + /** + * Constructor + */ + sSegment(const sMarker& start, const sMarker& end, float length_, const sFieldSegment* fieldSegment_) + : point{start,end}, length(length_), fieldSegment(fieldSegment_) {}; + + }; + + struct sFixedMarker { + bool visible; + Vector3f relPosPolar; + Vector3f relPosCart; + + /** + * Default constructor + */ + sFixedMarker() : relPosPolar(Vector3f()), relPosCart(Vector3f()), visible(false) {}; + + void set_relPos(Vector3f relPosPolar_){ + relPosPolar = relPosPolar_; + relPosCart = relPosPolar_.toCartesian(); + } + + }; + +//================================================================================================= +//================================================================================= Field constants +//================================================================================================= + + /** + * Constant field dimensions + */ + static constexpr double cFieldLength = 30.0, cFieldWidth = 20.0, cPenaltyLength = 1.8, cPenaltyWidth = 6.0; + static constexpr double cGoalWidth = 2.1, cGoalDepth = 0.6, cGoalHeight = 0.8; + + static constexpr double cHalfFielfLength = cFieldLength/2.0, cHalfFieldWidth = cFieldWidth/2.0; + static constexpr double cHalfGoalWidth = cGoalWidth/2.0, cHalfPenaltyLength = cPenaltyLength/2.0; + static constexpr double cHalfPenaltyWidth = cPenaltyWidth/2.0; + + static constexpr double cPenaltyBoxDistX = cHalfFielfLength-cPenaltyLength; + static constexpr double cRingLineLength = 1.2360679774997897; + + static constexpr float cHalfHorizontalFoV = 60; + static constexpr float cHalfVerticalFoV = 60; + + static constexpr float stdev_distance = 0.0965; //st. deviation of error ed (distance error=d/100*ed) + static constexpr float var_distance = 0.00931225; // variance of error ed (distance error=d/100*ed) + static constexpr float var_round_hundredth = 0.01*0.01/12; //variance of uniformly distributed random variable [-0.005,0.005] + + + class cFieldPoints{ + public: + /** + * Constant list of field points + * Notation + * "PT1-.-PT2" midpoint between PT1 and PT2 (2D/3D) + * "PT1-PT2" point between PT1 and PT2 (in 2D only) + */ + static constexpr std::array list {{ + {-cHalfFielfLength,-cHalfGoalWidth, cGoalHeight, "post--"}, {-cHalfFielfLength, cHalfGoalWidth, cGoalHeight, "post-+"}, //Goalposts x<0 + { cHalfFielfLength,-cHalfGoalWidth, cGoalHeight, "post+-"}, { cHalfFielfLength, cHalfGoalWidth, cGoalHeight, "post++"}, //Goalposts x>0 + {-cHalfFielfLength,-cHalfFieldWidth,0, "corner--"}, {-cHalfFielfLength, cHalfFieldWidth,0, "corner-+"}, //Corners x<0 + { cHalfFielfLength,-cHalfFieldWidth,0, "corner+-"}, { cHalfFielfLength, cHalfFieldWidth,0, "corner++"}, //Corners x>0 + {0,-cHalfFieldWidth, 0, "halfway-"}, // Halfway line ending y<0 + {0, cHalfFieldWidth, 0, "halfway+"}, // Halfway line ending y>0 + {-cHalfFielfLength, -cHalfPenaltyWidth, 0, "boxBack--"}, {-cHalfFielfLength, cHalfPenaltyWidth, 0, "boxBack-+"}, //Penalty box goal line corner x<0 + { cHalfFielfLength, -cHalfPenaltyWidth, 0, "boxBack+-"}, { cHalfFielfLength, cHalfPenaltyWidth, 0, "boxBack++"}, //Penalty box goal line corner x>0 + {-cPenaltyBoxDistX, -cHalfPenaltyWidth, 0, "boxFrnt--"}, {-cPenaltyBoxDistX, cHalfPenaltyWidth, 0, "boxFrnt-+"}, //Penalty box front corner x<0 + { cPenaltyBoxDistX, -cHalfPenaltyWidth, 0, "boxFrnt+-"}, { cPenaltyBoxDistX, cHalfPenaltyWidth, 0, "boxFrnt++"}, //Penalty box front corner x>0 + {2, 0, 0, "r0"}, { 1.6180339887498948, 1.1755705045849463, 0, "r36" }, //(18,19) Ring 0/36 deg + {0.61803398874989485, 1.9021130325903071, 0, "r72" }, {-0.61803398874989485, 1.9021130325903071, 0, "r108"}, //(20,21) Ring 72/108 deg + {-1.6180339887498948, 1.1755705045849463, 0, "r144"}, {-2, 0, 0, "r180"}, //(22,23) Ring 144/180 deg + {-1.6180339887498948, -1.1755705045849463, 0, "r216"}, {-0.61803398874989485, -1.9021130325903071, 0, "r252"}, //(24,25) Ring 216/252 deg + {0.61803398874989485, -1.9021130325903071, 0, "r288"}, { 1.6180339887498948, -1.1755705045849463, 0, "r324"} //(26,27) Ring 288/324 deg + }}; + + static constexpr const sFieldPoint &goal_mm = list[0]; //Goalpost x<0 y<0 + static constexpr const sFieldPoint &goal_mp = list[1]; //Goalpost x<0 y>0 + static constexpr const sFieldPoint &goal_pm = list[2]; //Goalpost x>0 y<0 + static constexpr const sFieldPoint &goal_pp = list[3]; //Goalpost x>0 y>0 + + static constexpr const sFieldPoint &corner_mm = list[4]; //Corner x<0 y<0 + static constexpr const sFieldPoint &corner_mp = list[5]; //Corner x<0 y>0 + static constexpr const sFieldPoint &corner_pm = list[6]; //Corner x>0 y<0 + static constexpr const sFieldPoint &corner_pp = list[7]; //Corner x>0 y>0 + + static constexpr const sFieldPoint &halfway_m = list[8]; //Halfway line ending y<0 + static constexpr const sFieldPoint &halfway_p = list[9]; //Halfway line ending y>0 + + static constexpr const sFieldPoint &boxgoal_mm = list[10]; //Penalty box goal line corner x<0 y<0 + static constexpr const sFieldPoint &boxgoal_mp = list[11]; //Penalty box goal line corner x<0 y>0 + static constexpr const sFieldPoint &boxgoal_pm = list[12]; //Penalty box goal line corner x>0 y<0 + static constexpr const sFieldPoint &boxgoal_pp = list[13]; //Penalty box goal line corner x>0 y>0 + + static constexpr const sFieldPoint &box_mm = list[14]; //Penalty box front corner x<0 y<0 + static constexpr const sFieldPoint &box_mp = list[15]; //Penalty box front corner x<0 y>0 + static constexpr const sFieldPoint &box_pm = list[16]; //Penalty box front corner x>0 y<0 + static constexpr const sFieldPoint &box_pp = list[17]; //Penalty box front corner x>0 y>0 + + static constexpr const sFieldPoint *rings = &list[18]; //iterator for 10 ring points + + }; + + + + /** + * Constant list of field line segments + * Each line segment has 3 characteristics: {startc, endc, length, angle, print name}, + * The angle is always positive, in [0,180[, and corresponds to the vector defined by (end-start) + * The print name should be used for printing purposes only, + * since the line segment can be identified by its constant index or address + */ + + class cFieldLineSegments{ + public: + static constexpr double c0deg = 0, c36deg = 0.62831853071795865, c72deg = 1.2566370614359173; + static constexpr double c90deg = 1.5707963267948966, c108deg = 1.8849555921538759, c144deg = 2.5132741228718346; + + static constexpr std::array list {{ + {&cFieldPoints::corner_mm, &cFieldPoints::corner_pm, cFieldLength, c0deg , "side-"}, // Sideline y<0 + {&cFieldPoints::corner_mp, &cFieldPoints::corner_pp, cFieldLength, c0deg , "side+"}, // Sideline y>0 + {&cFieldPoints::corner_mm, &cFieldPoints::corner_mp, cFieldWidth, c90deg , "goal-"}, // Goal line x<0 + {&cFieldPoints::corner_pm, &cFieldPoints::corner_pp, cFieldWidth, c90deg , "goal+"}, // Goal line x>0 + {&cFieldPoints::halfway_m, &cFieldPoints::halfway_p, cFieldWidth, c90deg , "halfway"},// Halfway line + {&cFieldPoints::boxgoal_mm, &cFieldPoints::box_mm, cPenaltyLength, c0deg , "box--"}, // Penalty box sideline x<0 y<0 + {&cFieldPoints::boxgoal_mp, &cFieldPoints::box_mp, cPenaltyLength, c0deg , "box-+"}, // Penalty box sideline x<0 y>0 + {&cFieldPoints::box_pm, &cFieldPoints::boxgoal_pm, cPenaltyLength, c0deg , "box+-"}, // Penalty box sideline x>0 y<0 + {&cFieldPoints::box_pp, &cFieldPoints::boxgoal_pp, cPenaltyLength, c0deg , "box++"}, // Penalty box sideline x>0 y>0 + {&cFieldPoints::box_mm, &cFieldPoints::box_mp, cPenaltyWidth, c90deg , "box-"}, // Penalty box front line x<0 + {&cFieldPoints::box_pm, &cFieldPoints::box_pp, cPenaltyWidth, c90deg , "box+"}, // Penalty box front line x>0 + {&cFieldPoints::rings[0], &cFieldPoints::rings[1], cRingLineLength, c108deg, "rL0"}, // Ring line 0 -> 36 + {&cFieldPoints::rings[1], &cFieldPoints::rings[2], cRingLineLength, c144deg, "rL1"}, // Ring line 36 -> 72 + {&cFieldPoints::rings[3], &cFieldPoints::rings[2], cRingLineLength, c0deg , "rL2"}, // Ring line 72 <- 108 + {&cFieldPoints::rings[4], &cFieldPoints::rings[3], cRingLineLength, c36deg , "rL3"}, // Ring line 108 <- 144 + {&cFieldPoints::rings[5], &cFieldPoints::rings[4], cRingLineLength, c72deg , "rL4"}, // Ring line 144 <- 180 + {&cFieldPoints::rings[6], &cFieldPoints::rings[5], cRingLineLength, c108deg, "rL5"}, // Ring line 180 <- 216 + {&cFieldPoints::rings[7], &cFieldPoints::rings[6], cRingLineLength, c144deg, "rL6"}, // Ring line 216 <- 252 + {&cFieldPoints::rings[7], &cFieldPoints::rings[8], cRingLineLength, c0deg , "rL7"}, // Ring line 252 -> 288 + {&cFieldPoints::rings[8], &cFieldPoints::rings[9], cRingLineLength, c36deg , "rL8"}, // Ring line 288 -> 324 + {&cFieldPoints::rings[9], &cFieldPoints::rings[0], cRingLineLength, c72deg , "rL9"} // Ring line 324 -> 0 + }}; + + static constexpr const sFieldSegment &side_m = list[0]; // Sideline y<0 + static constexpr const sFieldSegment &side_p = list[1]; // Sideline y>0 + static constexpr const sFieldSegment &goal_m = list[2]; // Goal line x<0 + static constexpr const sFieldSegment &goal_p = list[3]; // Goal line x>0 + + static constexpr const sFieldSegment &halfway = list[4]; //Halfway line + + static constexpr const sFieldSegment &box_mm = list[5]; // Penalty box sideline x<0 y<0 + static constexpr const sFieldSegment &box_mp = list[6]; // Penalty box sideline x<0 y>0 + static constexpr const sFieldSegment &box_pm = list[7]; // Penalty box sideline x>0 y<0 + static constexpr const sFieldSegment &box_pp = list[8]; // Penalty box sideline x>0 y>0 + + static constexpr const sFieldSegment &box_m = list[9]; // Penalty box front line x<0 + static constexpr const sFieldSegment &box_p = list[10]; // Penalty box front line x>0 + + static constexpr const sFieldSegment *rings = &list[11]; //iterator for 10 ring lines + + }; + + + + sSegment* get_known_segment(const sFieldSegment &id){ + for( auto& s : list_known_segments){ + if(s.fieldSegment == &id) return &s; + } + return nullptr; + } + +//================================================================================================= +//================================================================================= Control methods +//================================================================================================= + + /** + * Update markers, based on existing landmarks and lines + */ + void update(); + + /** + * Update markers, based on transformation matrix and existing lines + */ + void update_from_transformation(const Matrix4D& tmatrix); + + /** + * Update the absolute position of unknown markers, based on transformation matrix and existing lines + */ + void update_unknown_markers(const Matrix4D& tmatrix); + + /** + * Draw estimates of all visible lines, markers, self position and ball + */ + void draw_visible(const Matrix4D& headToFieldT, bool is_right_side) const; + + /** + * Draw estimates of all visible lines, markers, self position and ball, but switch field sides + */ + void draw_visible_switch(const Matrix4D& headToFieldT) const; + + +//================================================================================================= +//============================================================================= Visible collections +//================================================================================================= + + + /** + * Visible landmarks: corners + goalposts + */ + vector list_landmarks; + + /** + * Visible corners + */ + vector list_landmarks_corners; + + /** + * Visible goalposts + */ + vector list_landmarks_goalposts; + + /** + * Identified visible line segments + * Their start and endpoints' order is the same as the corresponding field segment to which they point + */ + vector list_known_segments; + + /** + * Identified visible line segment endpoints + landmarks + * Each marker has a reference to the corresponding field point + */ + vector list_known_markers; + + /** + * Endpoints (of identified visible line segments) whose corresponding field point is unknown + * Each marker has a reference to the corresponding field segment + * Note: list_known_markers + list_unknown_markers excludes points from unknown line segments + */ + vector list_unknown_markers; + + /** + * Visible line endpoints + foot contact points + corner flags (the absolute position is always (0,0,0)) + */ + vector list_ground_markers; + + /** + * Number of visible non-collinear (line endpoints + foot contact points) + * Note: collinearity between lines is impossible; between feet<->lines it is possible but unlikely + */ + int non_collinear_ground_markers; + + /** + * Same as list_ground_markers but closer points are repeated more often (proportional to distance) + */ + vector list_weighted_ground_markers; + + /** + * Feet contact points + */ + vector list_feet_contact_points; + + /** + * Visible line segments + */ + vector list_segments; + + /** + * Redundant list of all 8 landmarks' relative cartesian coordinates (to speed up lookups) + * It's different from world.landmarks since it is ordered by position, not by name, and it holds the cartesian relPos + * (this ordering difference is important when the teams switch sides) + */ + + class list_8_landmarks{ + friend class Field; + private: + static sFixedMarker list[8]; + //static std::array list; + static constexpr sFixedMarker &_corner_mm = list[0]; + static constexpr sFixedMarker &_corner_mp = list[1]; + static constexpr sFixedMarker &_corner_pm = list[2]; + static constexpr sFixedMarker &_corner_pp = list[3]; + static constexpr sFixedMarker &_goal_mm = list[4]; + static constexpr sFixedMarker &_goal_mp = list[5]; + static constexpr sFixedMarker &_goal_pm = list[6]; + static constexpr sFixedMarker &_goal_pp = list[7]; + public: + static constexpr const sFixedMarker &corner_mm = list[0]; + static constexpr const sFixedMarker &corner_mp = list[1]; + static constexpr const sFixedMarker &corner_pm = list[2]; + static constexpr const sFixedMarker &corner_pp = list[3]; + static constexpr const sFixedMarker &goal_mm = list[4]; + static constexpr const sFixedMarker &goal_mp = list[5]; + static constexpr const sFixedMarker &goal_pm = list[6]; + static constexpr const sFixedMarker &goal_pp = list[7]; + }; + + + +//================================================================================================= +//================================================================================== Math Utilities +//================================================================================================= + + /** + * Compute 3D distance between field line segment and cartesian point + * Field lines are on the ground (z=0), so the method is simplified + */ + static float fieldLineSegmentDistToCartPoint(const sFieldSegment& fLine, const Vector3f& cp); + + /** + * Compute 2D distance between field line segment and cartesian point + * Field lines are on the ground (z=0), so the method is simplified + */ + static float fieldLineSegmentDistToCart2DPoint(const sFieldSegment& fLine, const Vector& cp); + + /** + * Normalize angle between 2 lines + * @return angle between 0 and 90 deg + */ + static inline float normalize_line_angle_deg(float deg){ + return 90.f-fabsf(fmodf(fabsf(deg), 180.f) - 90.f); + } + + /** + * Normalize angle between 2 lines + * @return angle between 0 and pi/2 rad + */ + static inline float normalize_line_angle_rad(float rad){ + return 1.57079633f-fabsf(fmod(fabsf(rad), 3.14159265f) - 1.57079633f); + } + + /** + * Normalize angle between 2 vectors + * @return angle between 0 and 180 deg + */ + static inline float normalize_vector_angle_deg(float deg){ + return 180.f-fabsf(fmodf(fabsf(deg), 360.f) - 180.f); + } + + /** + * Normalize angle between 2 vectors + * @return angle between 0 and pi rad + */ + static inline float normalize_vector_angle_rad(float rad){ + return 3.14159265f-fabsf(fmod(fabsf(rad), 6.28318531f) - 3.14159265f); + } + +}; + +typedef Singleton SField; \ No newline at end of file diff --git a/cpp/localization/FieldNoise.cpp b/cpp/localization/FieldNoise.cpp new file mode 100644 index 0000000..9fb992f --- /dev/null +++ b/cpp/localization/FieldNoise.cpp @@ -0,0 +1,104 @@ +#include "FieldNoise.h" + +double FieldNoise::log_prob_r(double d, double r){ + double c1 = 100.0 * ((r-0.005)/d - 1); + double c2 = 100.0 * ((r+0.005)/d - 1); + return log_prob_normal_distribution(0, 0.0965, c1, c2); +} + +double FieldNoise::log_prob_h(double h, double phi){ + double c1 = phi - 0.005 - h; + double c2 = phi + 0.005 - h; + return log_prob_normal_distribution(0, 0.1225, c1, c2); +} + +double FieldNoise::log_prob_v(double v, double theta){ + double c1 = theta - 0.005 - v; + double c2 = theta + 0.005 - v; + return log_prob_normal_distribution(0, 0.1480, c1, c2); +} + + + +double FieldNoise::log_prob_normal_distribution(double mean, double std, double interval1, double interval2){ + const double std2 = std * sqrt(2); + double erf1_x = (mean - interval1)/std2; //lowest interval, highest expression + double erf2_x = (mean - interval2)/std2; //highest interval, lowest expression + + /** + * Computing erf(erf_x1) - erf(erf_x2) is the same as erf(erf_x1) + erf(-erf_x2). + * Intuitively, the former seems more natural. + * So, computationally, the former expression is accurate in the following situations: + * - erf_x1 * erf_x2 <= 0 + * - |erf_x1| < 3 _or_ |erf_x2| < 3 ('3' is just a ballpark figure, not really relevant) + * + * Known issues: erf(6.5)-erf(6.0) = 1-1 = 0 (actual result: 2.148e-17) + * Using 128b functions only mitigates the issue, which is quite common actually. + * + * For these cases, erf_aux(x) is used, although it is not precise for |x|<1. + * - erf_aux(x) allows the computation of erf(6.5)-erf(6.0) with 7 digits of precision + * - erf_aux(x) allows the computation of erf(8.5)-erf(8.0) with 3 digits of precision + * - erf(12.5)-erf(12) = 0 (which is not good for probability comparison) + * - erf_aux(x) allows the computation of log(erf(6.5)-erf(6.0)) with 8 digits of precision + * - erf_aux(x) allows the computation of log(erf(8.5)-erf(8.0)) with 5 digits of precision + * - log(erf(12.5)-erf(12)) = -4647 (real: -147) (not accurate but good for comparisons) + * + * The complete algorithm below that uses erf_aux(x) is almost as fast as the one which uses erf() from math.h (+30% runtime) + */ + + const double log05 = log(0.5); + + //If they have different sign or |erf1_x|<1 || |erf2_x|<1 + if( fabs(erf1_x) < 2 || fabs(erf2_x) < 2 || ((erf1_x > 0) ^ (erf2_x > 0))){ + return log( erf(erf1_x) - erf(erf2_x) ) + log05; // same but faster than log( 0.5 * (erf(erf1_x) - erf(erf2_x)) ) + } + + //Otherwise use erf_aux(x) + //At this point, erf1_x and erf2_x have the same sign and are both distant from 0 + + double erf1 = erf_aux(erf1_x); + double erf2 = erf_aux(erf2_x); + + //These operations are described in the documentation of erf_aux() + if(erf1_x > 0){ //both are positive + return log( 1.0 - exp(erf1-erf2) ) + erf2 + log05; + }else{ //both are negative + return log( 1.0 - exp(erf2-erf1) ) + erf1 + log05; + } + +} + + + +double FieldNoise::erf_aux(double a){ + double r, s, t, u; + + t = fabs (a); + s = a * a; + + r = fma (-5.6271698458222802e-018, t, 4.8565951833159269e-016); + u = fma (-1.9912968279795284e-014, t, 5.1614612430130285e-013); + r = fma (r, s, u); + r = fma (r, t, -9.4934693735334407e-012); + r = fma (r, t, 1.3183034417266867e-010); + r = fma (r, t, -1.4354030030124722e-009); + r = fma (r, t, 1.2558925114367386e-008); + r = fma (r, t, -8.9719702096026844e-008); + r = fma (r, t, 5.2832013824236141e-007); + r = fma (r, t, -2.5730580226095829e-006); + r = fma (r, t, 1.0322052949682532e-005); + r = fma (r, t, -3.3555264836704290e-005); + r = fma (r, t, 8.4667486930270974e-005); + r = fma (r, t, -1.4570926486272249e-004); + r = fma (r, t, 7.1877160107951816e-005); + r = fma (r, t, 4.9486959714660115e-004); + r = fma (r, t, -1.6221099717135142e-003); + r = fma (r, t, 1.6425707149019371e-004); + r = fma (r, t, 1.9148914196620626e-002); + r = fma (r, t, -1.0277918343487556e-001); + r = fma (r, t, -6.3661844223699315e-001); + r = fma (r, t, -1.2837929411398119e-001); + r = fma (r, t, -t); + + return r; +} \ No newline at end of file diff --git a/cpp/localization/FieldNoise.h b/cpp/localization/FieldNoise.h new file mode 100644 index 0000000..9d91c49 --- /dev/null +++ b/cpp/localization/FieldNoise.h @@ -0,0 +1,91 @@ +/** + * FILENAME: FieldNoise + * DESCRIPTION: efficient computation of relative probabilities (for the noise model of the RoboCup 3DSSL) + * AUTHOR: Miguel Abreu (m.abreu@fe.up.pt) + * DATE: 2021 + */ + +#pragma once +#include "math.h" + +class FieldNoise{ + public: + + /** + * Log probability of real distance d, given noisy radius r + */ + static double log_prob_r(double d, double r); + + /** + * Log probability of real horizontal angle h, given noisy angle phi + */ + static double log_prob_h(double h, double phi); + + /** + * Log probability of real vertical angle v, given noisy angle theta + */ + static double log_prob_v(double v, double theta); + + + private: + + FieldNoise(){}; //Disable construction + + /** + * Log probability of normally distributed random variable X from interval1 to interval2: + * Log Pr[interval1 < X < interval2] + * @param mean mean of random variable + * @param std standard deviation of random variable + * @param interval1 minimum value of random variable + * @param interval2 maximum value of random variable + */ + static double log_prob_normal_distribution(double mean, double std, double interval1, double interval2); + + /** + * This function returns ln(1-sgn(a)*erf(a)), but sgn(a)*erf(a) = |erf(a)|, because sgn(a) == sgn(erf(a)) + * So, it returns: ln(1-|erf(a)|), which is <=0 + * + * NOTE: condition to guarantee high precision: |a|>= 1 + * + * how to compute erf(a) ? + * erf(a) = sgn(a)(1-e^erf_aux(a)) + * + * how to compute erf(a)+erf(b) ? + * erf(a)+erf(b) = sgn(a)(1-e^erf_aux(a)) + sgn(b)(1-e^erf_aux(b)) + * assuming a<0 and b>0: + * = e^erf_aux(a) -1 + 1 - e^erf_aux(b) + * = e^erf_aux(a) - e^erf_aux(b) + * + * example: erf(-7)+erf(7.1) + * if we computed it directly: + * erf(-7)+erf(7.1) = -0.9999999(...) + 0.9999999(...) = -1+1 = 0 (due to lack of precision, even if using double) + * if we use the proposed method: + * e^erf_aux(-7) - e^erf_aux(7.1) = -1.007340e-23 - -4.183826e-23 = 3.176486E-23 + * + * how to compute ln(erf(a)+erf(b)) ? + * assuming a<0 and b>0: + * ln(erf(a)+erf(b)) = ln( exp(erf_aux(a)) - exp(erf_aux(b)) ) + * = ln( exp(erf_aux(a)-k) - exp(erf_aux(b)-k) ) + k + * where k = min(erf_aux(a), erf_aux(b)) + * + * how to compute ln(erf(a)-erf(b)) ? (the difference is just the assumption) + * assuming a*b >= 0 + * + * ln(erf(a)-erf(b)) = ln( sgn(a)(1-e^erf_aux(a)) - sgn(a)(1-e^erf_aux(b)) ), note that sgn(a)=sgn(b) + * + * rule: log( exp(a) - exp(b) ) = log( exp(a-k) - exp(b-k) ) + k + * + * if(a>0) + * ln(erf(a)-erf(b)) = ln( 1 - e^erf_aux(a) - 1 + e^erf_aux(b)) + * = ln( exp(erf_aux(b)) - exp(erf_aux(a)) ) + * = ln( exp(erf_aux(b)-erf_aux(b)) - exp(erf_aux(a)-erf_aux(b)) ) + erf_aux(b) + * = ln( 1 - exp(erf_aux(a)-erf_aux(b)) ) + erf_aux(b) + * if(a<0) + * ln(erf(a)-erf(b)) = ln( -1 + e^erf_aux(a) + 1 - e^erf_aux(b)) + * = ln( exp(erf_aux(a)) - exp(erf_aux(b)) ) + * = ln( exp(erf_aux(a)-erf_aux(a)) - exp(erf_aux(b)-erf_aux(a)) ) + erf_aux(a) + * = ln( 1 - exp(erf_aux(b)-erf_aux(a)) ) + erf_aux(a) + * + */ + static double erf_aux(double a); +}; \ No newline at end of file diff --git a/cpp/localization/Geometry.cpp b/cpp/localization/Geometry.cpp new file mode 100644 index 0000000..79f19ac --- /dev/null +++ b/cpp/localization/Geometry.cpp @@ -0,0 +1,344 @@ +#include "Geometry.h" + + +/** + * This function returns the cosine of a given angle in degrees using the + * built-in cosine function that works with angles in radians. + * + * @param x an angle in degrees + * @return the cosine of the given angle + */ +float Cos(float x) { + return ( cos(x * M_PI / 180)); +} + +/** + * This function returns the sine of a given angle in degrees using the + * built-in sine function that works with angles in radians. + * + * @param x an angle in degrees + * @return the sine of the given angle + */ +float Sin(float x) { + return ( sin(x * M_PI / 180)); +} + +/** + * This function returns the principal value of the arc tangent of y/x in + * degrees using the signs of both arguments to determine the quadrant of the + * return value. For this the built-in 'atan2' function is used which returns + * this value in radians. + * + * @param x a float value + * @param y a float value + * @return the arc tangent of y/x in degrees taking the signs of x and y into + * account + */ +float ATan2(float x, float y) { + if (fabs(x) < EPSILON && fabs(y) < EPSILON) + return ( 0.0); + + return ( atan2(x, y) * 180 / M_PI ); +} + + + + +/************************************************************************/ +/******************* CLASS VECTOR ***********************************/ +/************************************************************************/ + +/*! Constructor for the Vector class. Arguments x and y + denote the x- and y-coordinates of the new position. + \param x the x-coordinate of the new position + \param y the y-coordinate of the new position + \return the Vector corresponding to the given arguments */ +Vector::Vector(float vx, float vy) : x(vx), y(vy) {} + +/*! Overloaded version of unary minus operator for Vectors. It returns the + negative Vector, i.e. both the x- and y-coordinates are multiplied by + -1. The current Vector itself is left unchanged. + \return a negated version of the current Vector */ +Vector Vector::operator-() const{ + return ( Vector(-x, -y)); +} + +/*! Overloaded version of the binary plus operator for adding a given float + value to a Vector. The float value is added to both the x- and + y-coordinates of the current Vector. The current Vector itself is + left unchanged. + \param d a float value which has to be added to both the x- and + y-coordinates of the current Vector + \return the result of adding the given float value to the current + Vector */ +Vector Vector::operator+(const float &d) const{ + return ( Vector(x + d, y + d)); +} + +/*! Overloaded version of the binary plus operator for Vectors. It returns + the sum of the current Vector and the given Vector by adding their + x- and y-coordinates. The Vectors themselves are left unchanged. + \param p a Vector + \return the sum of the current Vector and the given Vector */ +Vector Vector::operator+(const Vector &p) const{ + return ( Vector(x + p.x, y + p.y)); +} + +/*! Overloaded version of the binary minus operator for subtracting a + given float value from a Vector. The float value is + subtracted from both the x- and y-coordinates of the current + Vector. The current Vector itself is left unchanged. + \param d a float value which has to be subtracted from both the x- and + y-coordinates of the current Vector + \return the result of subtracting the given float value from the current + Vector */ +Vector Vector::operator-(const float &d) const{ + return ( Vector(x - d, y - d)); +} + +/*! Overloaded version of the binary minus operator for + Vectors. It returns the difference between the current + Vector and the given Vector by subtracting their x- and + y-coordinates. The Vectors themselves are left unchanged. + + \param p a Vector + \return the difference between the current Vector and the given + Vector */ +Vector Vector::operator-(const Vector &p) const { + return ( Vector(x - p.x, y - p.y)); +} + +/*! Overloaded version of the multiplication operator for multiplying a + Vector by a given float value. Both the x- and y-coordinates of the + current Vector are multiplied by this value. The current Vector + itself is left unchanged. + \param d the multiplication factor + \return the result of multiplying the current Vector by the given + float value */ +Vector Vector::operator*(const float &d) const{ + return ( Vector(x * d, y * d)); +} + +/*! Overloaded version of the multiplication operator for + Vectors. It returns the product of the current Vector + and the given Vector by multiplying their x- and + y-coordinates. The Vectors themselves are left unchanged. + + \param p a Vector + \return the product of the current Vector and the given Vector */ +Vector Vector::operator*(const Vector &p) const{ + return ( Vector(x * p.x, y * p.y)); +} + +/*! Overloaded version of the division operator for dividing a + Vector by a given float value. Both the x- and y-coordinates + of the current Vector are divided by this value. The current + Vector itself is left unchanged. + + \param d the division factor + \return the result of dividing the current Vector by the given float + value */ +Vector Vector::operator/(const float &d) const{ + return ( Vector(x / d, y / d)); +} + +/*! Overloaded version of the division operator for Vectors. It + returns the quotient of the current Vector and the given + Vector by dividing their x- and y-coordinates. The + Vectors themselves are left unchanged. + + \param p a Vector + \return the quotient of the current Vector and the given one */ +Vector Vector::operator/(const Vector &p) const{ + return ( Vector(x / p.x, y / p.y)); +} + +/*! Overloaded version of the assignment operator for assigning a given float + value to both the x- and y-coordinates of the current Vector. This + changes the current Vector itself. + \param d a float value which has to be assigned to both the x- and + y-coordinates of the current Vector */ +void Vector::operator=(const float &d) { + x = d; + y = d; +} + +/*! Overloaded version of the sum-assignment operator for Vectors. It + returns the sum of the current Vector and the given Vector by + adding their x- and y-coordinates. This changes the current Vector + itself. + \param p a Vector which has to be added to the current Vector */ +void Vector::operator+=(const Vector &p) { + x += p.x; + y += p.y; +} + +/*! Overloaded version of the sum-assignment operator for adding a given float + value to a Vector. The float value is added to both the x- and + y-coordinates of the current Vector. This changes the current + Vector itself. + \param d a float value which has to be added to both the x- and + y-coordinates of the current Vector */ +void Vector::operator+=(const float &d) { + x += d; + y += d; +} + +/*! Overloaded version of the difference-assignment operator for + Vectors. It returns the difference between the current + Vector and the given Vector by subtracting their x- and + y-coordinates. This changes the current Vector itself. + + \param p a Vector which has to be subtracted from the current + Vector */ +void Vector::operator-=(const Vector &p) { + x -= p.x; + y -= p.y; +} + +/*! Overloaded version of the difference-assignment operator for + subtracting a given float value from a Vector. The float + value is subtracted from both the x- and y-coordinates of the + current Vector. This changes the current Vector itself. + + \param d a float value which has to be subtracted from both the x- and + y-coordinates of the current Vector */ +void Vector::operator-=(const float &d) { + x -= d; + y -= d; +} + +/*! Overloaded version of the multiplication-assignment operator for + Vectors. It returns the product of the current Vector + and the given Vector by multiplying their x- and + y-coordinates. This changes the current Vector itself. + + \param p a Vector by which the current Vector has to be + multiplied */ +void Vector::operator*=(const Vector &p) { + x *= p.x; + y *= p.y; +} + +/*! Overloaded version of the multiplication-assignment operator for + multiplying a Vector by a given float value. Both the x- and + y-coordinates of the current Vector are multiplied by this + value. This changes the current Vector itself. + + \param d a float value by which both the x- and y-coordinates of the + current Vector have to be multiplied */ +void Vector::operator*=(const float &d) { + x *= d; + y *= d; +} + +/*! Overloaded version of the division-assignment operator for + Vectors. It returns the quotient of the current Vector + and the given Vector by dividing their x- and + y-coordinates. This changes the current Vector itself. + + \param p a Vector by which the current Vector is divided */ +void Vector::operator/=(const Vector &p) { + x /= p.x; + y /= p.y; +} + +/*! Overloaded version of the division-assignment operator for + dividing a Vector by a given float value. Both the x- and + y-coordinates of the current Vector are divided by this + value. This changes the current Vector itself. + + \param d a float value by which both the x- and y-coordinates of the + current Vector have to be divided */ +void Vector::operator/=(const float &d) { + x /= d; + y /= d; +} + +/*! Overloaded version of the inequality operator for Vectors. It + determines whether the current Vector is unequal to the given + Vector by comparing their x- and y-coordinates. + + \param p a Vector + \return true when either the x- or y-coordinates of the given Vector + and the current Vector are different; false otherwise */ +bool Vector::operator!=(const Vector &p) { + return ( (x != p.x) || (y != p.y)); +} + +/*! Overloaded version of the inequality operator for comparing a + Vector to a float value. It determines whether either the x- + or y-coordinate of the current Vector is unequal to the given + float value. + + \param d a float value with which both the x- and y-coordinates of the + current Vector have to be compared. + \return true when either the x- or y-coordinate of the current Vector + is unequal to the given float value; false otherwise */ +bool Vector::operator!=(const float &d) { + return ( (x != d) || (y != d)); +} + +/*! Overloaded version of the equality operator for Vectors. It + determines whether the current Vector is equal to the given + Vector by comparing their x- and y-coordinates. + + \param p a Vector + \return true when both the x- and y-coordinates of the given + Vector and the current Vector are equal; false + otherwise */ +bool Vector::operator==(const Vector &p) { + return ( (x == p.x) && (y == p.y)); +} + +/*! Overloaded version of the equality operator for comparing a + Vector to a float value. It determines whether both the x- + and y-coordinates of the current Vector are equal to the + given float value. + + \param d a float value with which both the x- and y-coordinates of the + current Vector have to be compared. + \return true when both the x- and y-coordinates of the current Vector + are equal to the given float value; false otherwise */ +bool Vector::operator==(const float &d) { + return ( (x == d) && (y == d)); +} + + +/*! This method determines the distance between the current + Vector and a given Vector. This is equal to the + magnitude (length) of the vector connecting the two positions + which is the difference vector between them. + + \param p a Vecposition + \return the distance between the current Vector and the given + Vector */ +float Vector::getDistanceTo(const Vector p) { + return ( (*this -p).length()); +} + + +/*! This method determines the magnitude (length) of the vector + corresponding with the current Vector using the formula of + Pythagoras. + + \return the length of the vector corresponding with the current + Vector */ +float Vector::length() const { + return ( sqrt(x * x + y * y)); +} + +float Vector::crossProduct(const Vector p) { + return this->x*p.y - this->y*p.x; +} + + +/** + * This methods returns the inner product of this vector with another + * + * @param other the other vector + * @return inner product + */ +float Vector::innerProduct(const Vector& other) const { + return x * other.x + y * other.y; +} \ No newline at end of file diff --git a/cpp/localization/Geometry.h b/cpp/localization/Geometry.h new file mode 100644 index 0000000..c4afa07 --- /dev/null +++ b/cpp/localization/Geometry.h @@ -0,0 +1,71 @@ +#ifndef GEOMETRY_H +#define GEOMETRY_H + +#include +#include + +using namespace std; + +#define EPSILON 1e-10 + +/** + * Useful functions to operate with angles in degrees + */ +float Cos(float x); +float Sin(float x); +float ATan2(float x, float y); + + +/** + * @class Vector + * + * @brief This class represents a position in the 2d space + * + * A position is represented by a x-axis coordinate and a + * y-axis coordinate or in polar coordinates (r, phi) + * + * @author Hugo Picado (hugopicado@ua.pt) + * @author Nuno Almeida (nuno.alm@ua.pt) + * Adapted - Miguel Abreu + */ +class Vector { +public: + Vector(float vx = 0, float vy = 0); + + // overloaded arithmetic operators + Vector operator-() const; + Vector operator+(const float &d) const; + Vector operator+(const Vector &p) const; + Vector operator-(const float &d) const; + Vector operator-(const Vector &p) const; + Vector operator*(const float &d) const; + Vector operator*(const Vector &p) const; + Vector operator/(const float &d) const; + Vector operator/(const Vector &p) const; + void operator=(const float &d); + void operator+=(const Vector &p); + void operator+=(const float &d); + void operator-=(const Vector &p); + void operator-=(const float &d); + void operator*=(const Vector &p); + void operator*=(const float &d); + void operator/=(const Vector &p); + void operator/=(const float &d); + bool operator!=(const Vector &p); + bool operator!=(const float &d); + bool operator==(const Vector &p); + bool operator==(const float &d); + + float getDistanceTo(const Vector p); + float crossProduct(const Vector p); + float length() const; + float innerProduct(const Vector &p) const; + +public: + float x; + float y; +}; + + + +#endif // GEOMETRY_H diff --git a/cpp/localization/Line6f.cpp b/cpp/localization/Line6f.cpp new file mode 100644 index 0000000..ad41754 --- /dev/null +++ b/cpp/localization/Line6f.cpp @@ -0,0 +1,232 @@ +#include "Line6f.h" + + + +Line6f::Line6f(const Vector3f &polar_s, const Vector3f &polar_e) : + startp(polar_s), endp(polar_e), startc(polar_s.toCartesian()), endc(polar_e.toCartesian()), length(startc.dist(endc)) {}; + +Line6f::Line6f(const Vector3f &cart_s, const Vector3f &cart_e, float length) : + startp(cart_s.toPolar()), endp(cart_e.toPolar()), startc(cart_s), endc(cart_e), length(length) {}; + +Line6f::Line6f(const Line6f &obj) : + startp(obj.startp), endp(obj.endp), startc(obj.startc), endc(obj.endc), length(obj.length) {}; + +Vector3f Line6f::linePointClosestToCartPoint(const Vector3f &cp) const{ + + /** + * Equation of this line: (we want to find t, such that (cp-p) is perpendicular to this line) + * p = startc + t*(endc - startc) + * + * Let vecp=(cp-start) and vecline=(end-start) + * Scalar projection of vecp in the direction of vecline: + * sp = vecp.vecline/|vecline| + * Find the ratio t by dividing the scalar projection by the length of vecline: + * t = sp / |vecline| + * So the final expression becomes: + * t = vecp.vecline/|vecline|^2 + */ + + + Vector3f lvec(endc-startc); //this line's vector + float t = (cp-startc).innerProduct(lvec) / (length*length); + + return startc + lvec*t; +} + +Vector3f Line6f::linePointClosestToPolarPoint(const Vector3f &pp) const{ + return linePointClosestToCartPoint(pp.toCartesian()); +} + +float Line6f::lineDistToCartPoint(const Vector3f &cp) const{ + return ((cp-startc).crossProduct(cp-endc)).length() / (endc-startc).length(); +} + +float Line6f::lineDistToPolarPoint(const Vector3f &pp) const{ + return lineDistToCartPoint(pp.toCartesian()); +} + +//source: http://geomalgorithms.com/a07-_distance.html#dist3D_Line_to_Line() +float Line6f::lineDistToLine(const Line6f &l) const{ + Vector3f u = endc - startc; + Vector3f v = l.endc - l.startc; + Vector3f w = startc - l.startc; + float a = u.innerProduct(u); // always >= 0 + float b = u.innerProduct(v); + float c = v.innerProduct(v); // always >= 0 + float d = u.innerProduct(w); + float e = v.innerProduct(w); + float D = a*c - b*b; // always >= 0 + float sc, tc; + + // compute the line parameters of the two closest points + if (D < 1e-8f) { // the lines are almost parallel + sc = 0.0; + tc = (b>c ? d/b : e/c); // use the largest denominator + } + else { + sc = (b*e - c*d) / D; + tc = (a*e - b*d) / D; + } + + // get the difference of the two closest points + Vector3f dP = w + (u * sc) - (v * tc); // = L1(sc) - L2(tc) + + return dP.length(); // return the closest distance +} + +Vector3f Line6f::segmentPointClosestToCartPoint(const Vector3f &cp) const{ + + /** + * Equation of this line: (we want to find t, such that (cp-p) is perpendicular to this line) + * p = startc + t*(endc - startc) + * + * Let vecp=(cp-start) and vecline=(end-start) + * Scalar projection of vecp in the direction of vecline: + * sp = vecp.vecline/|vecline| + * Find the ratio t by dividing the scalar projection by the length of vecline: + * t = sp / |vecline| + * So the final expression becomes: + * t = vecp.vecline/|vecline|^2 + * Since this version requires that p belongs to the line segment, there is an additional restriction: + * 0 < t < 1 + */ + + Vector3f lvec(endc-startc); //this line's vector + float t = (cp-startc).innerProduct(lvec) / (length*length); + + if(t<0) t=0; + else if(t>1) t=1; + + return startc + lvec*t; +} + +Vector3f Line6f::segmentPointClosestToPolarPoint(const Vector3f &pp) const{ + return segmentPointClosestToCartPoint(pp.toCartesian()); +} + +float Line6f::segmentDistToCartPoint(const Vector3f &cp) const{ + + Vector3f v = endc - startc; //line segment vector + Vector3f w1 = cp - startc; + + if ( w1.innerProduct(v) <= 0) return w1.length(); + + Vector3f w2 = cp - endc; + + if ( w2.innerProduct(v) >= 0) return w2.length(); + + return v.crossProduct(w1).length() / this->length; + +} + +float Line6f::segmentDistToPolarPoint(const Vector3f &pp) const{ + return segmentDistToCartPoint(pp.toCartesian()); +} + +//source: http://geomalgorithms.com/a07-_distance.html#dist3D_Segment_to_Segment() +float Line6f::segmentDistToSegment(const Line6f &other) const{ + + Vector3f u = endc - startc; + Vector3f v = other.endc - other.startc; + Vector3f w = startc - other.startc; + float a = u.innerProduct(u); // always >= 0 + float b = u.innerProduct(v); + float c = v.innerProduct(v); // always >= 0 + float d = u.innerProduct(w); + float e = v.innerProduct(w); + float D = a*c - b*b; // always >= 0 + float sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 + float tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0 + + // compute the line parameters of the two closest points + if (D < 1e-8f) { // the lines are almost parallel + sN = 0.0; // force using point start on segment S1 + sD = 1.0; // to prevent possible division by 0.0 later + tN = e; + tD = c; + } + else { // get the closest points on the infinite lines + sN = (b*e - c*d); + tN = (a*e - b*d); + if (sN < 0.0) { // sc < 0 => the s=0 edge is visible + sN = 0.0; + tN = e; + tD = c; + } + else if (sN > sD) { // sc > 1 => the s=1 edge is visible + sN = sD; + tN = e + b; + tD = c; + } + } + + if (tN < 0.0) { // tc < 0 => the t=0 edge is visible + tN = 0.0; + // recompute sc for this edge + if (-d < 0.0) + sN = 0.0; + else if (-d > a) + sN = sD; + else { + sN = -d; + sD = a; + } + } + else if (tN > tD) { // tc > 1 => the t=1 edge is visible + tN = tD; + // recompute sc for this edge + if ((-d + b) < 0.0) + sN = 0; + else if ((-d + b) > a) + sN = sD; + else { + sN = (-d + b); + sD = a; + } + } + // finally do the division to get sc and tc + sc = (abs(sN) < 1e-8f ? 0.0 : sN / sD); + tc = (abs(tN) < 1e-8f ? 0.0 : tN / tD); + + // get the difference of the two closest points + Vector3f dP = w + (u * sc) - (v * tc); // = S1(sc) - S2(tc) + + return dP.length(); // return the closest distance + +} + + +Vector3f Line6f::midPointCart() const{ + return (startc+endc)/2; +} + +Vector3f Line6f::midPointPolar() const{ + return midPointCart().toPolar(); +} + + +bool Line6f::operator==(const Line6f& other) const { + return (startp == other.startp) && (endp == other.endp); +} + +const Vector3f &Line6f::get_polar_pt(const int index) const{ + if(index==0) return startp; + return endp; +} + +const Vector3f &Line6f::get_cart_pt(const int index) const{ + if(index==0) return startc; + return endc; +} + +Vector3f Line6f::get_polar_vector() const{ + return endp-startp; +} + +Vector3f Line6f::get_cart_vector() const{ + return endc-startc; +} + + + + diff --git a/cpp/localization/Line6f.h b/cpp/localization/Line6f.h new file mode 100644 index 0000000..03efecb --- /dev/null +++ b/cpp/localization/Line6f.h @@ -0,0 +1,185 @@ +/** + * FILENAME: Line6f + * DESCRIPTION: Simple line (segment) class + * AUTHOR: Miguel Abreu (m.abreu@fe.up.pt) + * DATE: 2021 + * + * Immutable Class for: + * 3D Line composed of 2 Vector3f points + * Optimized for: + * - regular access to cartesian coordinates + * - regular access to line segment length + **/ + +#pragma once +#include "Vector3f.h" + + +class Line6f { +public: + + /** + * Polar start coordinate + */ + const Vector3f startp; + + /** + * Polar end coordinate + */ + const Vector3f endp; + + /** + * Cartesian start coordinate + */ + const Vector3f startc; + + /** + * Cartesian end coordinate + */ + const Vector3f endc; + + /** + * Length of line segment limited by (start,end) + */ + const float length; + + /** + * Constructor + * @param polar_s polar start point + * @param polar_e polar end point + */ + Line6f(const Vector3f &polar_s, const Vector3f &polar_e); + + /** + * Constructor + * @param cart_s cartesian start point + * @param cart_e cartesian end point + * @param length line segment length + */ + Line6f(const Vector3f &cart_s, const Vector3f &cart_e, float length); + + /** + * Copy Constructor + * @param obj another line + */ + Line6f(const Line6f &obj); + + /** + * Find point in line defined by (start,end) closest to given point + * @param cp cartesian point + * @return point in this infinite line closest to given point + */ + Vector3f linePointClosestToCartPoint(const Vector3f &cp) const; + + /** + * Find point in line defined by (start,end) closest to given point + * @param pp polar point + * @return point in this infinite line closest to given point + */ + Vector3f linePointClosestToPolarPoint(const Vector3f &pp) const; + + /** + * Find distance between line defined by (start,end) and given point + * @param cp cartesian point + * @return distance between line defined by (start,end) and given point + */ + float lineDistToCartPoint(const Vector3f &cp) const; + + /** + * Find distance between line defined by (start,end) and given point + * @param pp polar point + * @return distance between line defined by (start,end) and given point + */ + float lineDistToPolarPoint(const Vector3f &pp) const; + + /** + * Find distance between line defined by (start,end) and given line + * @param l line + * @return distance between line defined by (start,end) and given line + */ + float lineDistToLine(const Line6f &l) const; + + /** + * Find point in line defined by (start,end) closest to given point + * @param cp cartesian point + * @return point in this infinite line closest to given point + */ + Vector3f segmentPointClosestToCartPoint(const Vector3f &cp) const; + + /** + * Find point in line defined by (start,end) closest to given point + * @param pp polar point + * @return point in this infinite line closest to given point + */ + Vector3f segmentPointClosestToPolarPoint(const Vector3f &pp) const; + + /** + * Find distance between line segment limited by (start,end) and given point + * @param cp cartesian point + * @return distance between line segment limited by (start,end) and given point + */ + float segmentDistToCartPoint(const Vector3f &cp) const; + + /** + * Find distance between line segment limited by (start,end) and given point + * @param pp polar point + * @return distance between line segment limited by (start,end) and given point + */ + float segmentDistToPolarPoint(const Vector3f &pp) const; + + /** + * Find distance between line segment limited by (start,end) and given line segment + * @param other line segment + * @return distance between line segment limited by (start,end) and given line segment + */ + float segmentDistToSegment(const Line6f &other) const; + + /** + * Find midpoint of line segment limited by (start,end) + * @return cartesian midpoint of line segment limited by (start,end) + */ + Vector3f midPointCart() const; + + /** + * Find midpoint of line segment limited by (start,end) + * @return polar midpoint of line segment limited by (start,end) + */ + Vector3f midPointPolar() const; + + /** + * Operator == + * @param other line + * @return true if both lines are the same + */ + bool operator==(const Line6f& other) const; + + /** + * Get polar line ending by index + * @param index (0)->start or (1)->end + * @return polar line ending according to given index + */ + const Vector3f &get_polar_pt(const int index) const; + + /** + * Get cartesian line ending by index + * @param index (0)->start or (1)->end + * @return cartesian line ending according to given index + */ + const Vector3f &get_cart_pt(const int index) const; + + /** + * Get polar vector (end-start) + * @return polar vector (end-start) + */ + Vector3f get_polar_vector() const; + + /** + * Get cartesian vector (end-start) + * @return cartesian vector (end-start) + */ + Vector3f get_cart_vector() const; + + + + +}; \ No newline at end of file diff --git a/cpp/localization/LocalizerV2.cpp b/cpp/localization/LocalizerV2.cpp new file mode 100644 index 0000000..5971e83 --- /dev/null +++ b/cpp/localization/LocalizerV2.cpp @@ -0,0 +1,1249 @@ +#include "LocalizerV2.h" +#include "math.h" +#include "iostream" +#include "World.h" + +using namespace std; + +static World& world = SWorld::getInstance(); + + +/** + * Compute 3D position and 3D orientation + * */ +void LocalizerV2::run(){ + + Field& fd = SField::getInstance(); + + stats_change_state(RUNNING); + + //------------------ WORKFLOW: 0 + + _is_uptodate = false; + _is_head_z_uptodate = false; + _steps_since_last_update++; + + prelim_reset(); //reset preliminary transformation matrix + + fd.update(); //update visible collections + int lines_no = fd.list_segments.size(); + int landmarks_no = fd.list_landmarks.size(); + + if( (landmarks_no == 0 && lines_no < 2) || (lines_no == 0) ){ + if(lines_no==0 && landmarks_no==0){ stats_change_state(BLIND); } else { stats_change_state(MINFAIL); } + return; + } + + //------------------ WORKFLOW: 1-2 + + if( ! find_z_axis_orient_vec() ){ return; } + + //------------------ WORKFLOW: 3-4 + + if(!( landmarks_no >1 ? find_xy() : guess_xy() )){ return; } + + //------------------ Update public variables + + commit_everything(); + + stats_change_state(DONE); + + //------------------ Statistics + + //Ball position stats + if(world.ball_seen){ + counter_ball += stats_sample_position_error(prelimHeadToField * world.ball_rel_pos_cart, + world.ball_cheat_abs_cart_pos, errorSum_ball); + } + + //print_report(); //uncomment to enable report (average errors + solution analysis) + +} + + + +//================================================================================================= +//=================================================================================== GSL Utilities +//================================================================================================= + + +void add_gsl_regression_sample(gsl_matrix* m, gsl_vector* v, int sample_no, const Vector3f& relativeCoord, double absoluteCoord, double translCoeffMult=1){ + + gsl_matrix_set(m, sample_no, 0, relativeCoord.x); + gsl_matrix_set(m, sample_no, 1, relativeCoord.y); + gsl_matrix_set(m, sample_no, 2, relativeCoord.z); + gsl_matrix_set(m, sample_no, 3, translCoeffMult); + gsl_vector_set(v, sample_no, absoluteCoord ); + +} + +template +gsl_vector* create_gsl_vector(const std::array &content){ + + gsl_vector* v = gsl_vector_alloc (SIZE); + + for(int i=0; i 0){ //if vec is (0,0,1) or (0,0,-1) we keep the default rotation axis + gx = -vec.y / aux; + gy = vec.x / aux; + } + + return Vector(gx,gy); +} + + +/** + * Rotate vector around ground axis defined as u=(0,0,1)x(Zvec)/|Zvec| + * Direction of rotation from Zvec to (0,0,1) + * To invert the rotation direction, invert Zvec.x and Zvec.y + * @param v vector to be rotated + * @param Zvec unit normal vector of rotated ground plane + */ +Vector3f fast_rotate_around_ground_axis(Vector3f v, Vector3f Zvec){ + + Vector u = get_ground_unit_vec_perpendicular_to(Zvec); + + //Angle between unit normal vector of original plane and unit normal vector of rotated plane: + //cos(a) = (ov.rv)/(|ov||rv|) = ((0,0,1).(rvx,rvy,rvz))/(1*1) = rvz + float& cos_a = Zvec.z; + //assert(cos_a <= 1); + if(cos_a > 1) cos_a = 1; //Fix: it happens rarely, no cause was yet detected + float sin_a = -sqrtf(1 - cos_a*cos_a); //invert sin_a to invert a (direction was defined in method description) + + const float i_cos_a = 1-cos_a; + const float uxuy_i = u.x*u.y*i_cos_a; + const float uxux_i = u.x*u.x*i_cos_a; + const float uyuy_i = u.y*u.y*i_cos_a; + const float uxsin_a = u.x*sin_a; + const float uysin_a = u.y*sin_a; + + float x = (cos_a + uxux_i ) * v.x + uxuy_i * v.y + uysin_a * v.z; + float y = uxuy_i * v.x + (cos_a + uyuy_i ) * v.y - uxsin_a * v.z; + float z = - uysin_a * v.x + uxsin_a * v.y + cos_a * v.z; + + return Vector3f(x,y,z); + +} + + +/** + * Compute Xvec and Yvec from Zvec and angle + * @param Zvec input z-axis orientation vector + * @param angle input rotation of Xvec around Zvec (rads) + * @param Xvec output x-axis orientation vector + * @param Yvec output y-axis orientation vector + */ +void fast_compute_XYvec_from_Zvec(const Vector3f& Zvec, float agent_angle, Vector3f& Xvec, Vector3f& Yvec){ + /** + * There are two coordinate systems being considered in this method: + * - The actual agent's vision (RELATIVE system -> RELsys) + * - A rotated perspective where the agent's seen Zvec is the real Zvec (ROTATED system -> ROTsys) + * (the agent's optical axis / line of sight is parallel to ground ) + * + * SUMMARY: + * I provide an angle which defines the agent's rotation around Zvec (in the ROTsys) + * E.g. suppose the agent is rotated 5deg, then in the ROTsys, the agent sees Xvec as being rotated -5deg + * I then rotate Xvec to the RELsys, and compute the Yvec using cross product + * + * STEPS: + * 1st. Compute ROTsys, by finding rotation of plane defined by normal vector Zvec, in relation to seen XY plane + * (whose seen normal vector is (0,0,1)). We need: axis of rotation (unit vector lying on XY plane) and angle (rads): + * rotation axis: + * u = (0,0,1)x(Zvec) (vector perpendicular to XY plane and Zvec) + * = (-ZvecY,ZvecX,0) (rotation will be counterclockwise when u points towards the observer) + * (so a negative angle will bring the agent to the ROTsys) + * angle between Zvec and (0,0,1): + * a = acos( ((0,0,1).(Zvec)) / (|(0,0,1)|*|Zvec|) ) + * = acos( ((0,0,1).(Zvec)) ) + * = acos( ZvecZ ) + * + * 2nd. Establish Xvec in ROTsys: + * Let agent_angle be the agent's angle. Then Xvec's angle is (b=-agent_angle). + * Xvec = (cos(b),sin(b),0) + * + * 3rd. Rotate Xvec to RELsys: + * Let R be the rotation matrix that rotates from ROTsys to RELsys (positive angle using u): + * Xvec = R * Xvec + * = R * (cos(b),sin(b),0) + * = (R00 * cos(b) + R01 * sin(b), R10 * cos(b) + R11 * sin(b), R20 * cos(b) + R21 * sin(b)) + * where R is: (rotation matrix from axis and angle https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle) + * R00 = cos(a) + ux*ux(1-cos(a)) R01 = ux*uy(1-cos(a)) + * R10 = uy*ux(1-cos(a)) R11 = cos(a) + uy*uy(1-cos(a)) + * R20 = -uy*sin(a) R21 = ux*sin(a) + * so Xvec becomes: + * XvecX = cos(a)*cos(b) + (1-cos(a))*(ux*ux*cos(b) + ux*uy*sin(b)) + * XvecY = cos(a)*sin(b) + (1-cos(a))*(uy*uy*sin(b) + ux*uy*cos(b)) + * XvecZ = sin(a)*(ux*sin(b) - uy*cos(b)) + * + * 4th. To find Yvec we have two options: + * A. add pi/2 to b and compute Yvec with the same expression used for Xvec + * B. Yvec = Zvec x Xvec (specifically in this order for original coordinate system) + */ + + Vector u = get_ground_unit_vec_perpendicular_to(Zvec); + + const float& cos_a = Zvec.z; + const float sin_a = sqrtf(1 - cos_a*cos_a); + const float uxuy = u.x * u.y; + const float b = -agent_angle; //Xvec's angle + const float cos_b = cosf(b); + const float sin_b = sinf(b); + const float i_cos_a = 1-cos_a; + + Xvec.x = cos_a * cos_b + i_cos_a * ( u.x*u.x*cos_b + uxuy*sin_b ); + Xvec.y = cos_a * sin_b + i_cos_a * ( u.y*u.y*sin_b + uxuy*cos_b ); + Xvec.z = sin_a * ( u.x*sin_b - u.y*cos_b ); + + Yvec = Zvec.crossProduct(Xvec); //Using original coordinate system +} + + +//================================================================================================= +//================================================================================== Main algorithm +//================================================================================================= + +/** + * WORKFLOW: 1 + * See workflow description on header + */ +bool LocalizerV2::find_z_axis_orient_vec(){ + + Field& fd = SField::getInstance(); + + const int goalNo = fd.list_landmarks_goalposts.size(); + + if(fd.non_collinear_ground_markers >= 3){ + fit_ground_plane(); + return true; + } + + /** + * At this point we have > 0 lines. Having 1 line is the most common option. + * But having 2 lines is also possible if at least one of them is too short to create 2 reference points. + * But those 2 lines can be both too short, which would not be ideal for the algorithms below. + * Steps: find largest line, check if it is large enough + */ + const Line6f* l = &fd.list_segments.front(); + if(fd.list_segments.size() > 1){ + for(const auto& ll: fd.list_segments){ + if(ll.length > l->length) l = ≪ + } + } + + if(l->length < 1){ //larger line is too short + stats_change_state(FAILzLine); + return false; + } + + if(goalNo == 0){ //Case e.g.: 1 corner and 1 line (or 2 short lines) + stats_change_state(FAILzNOgoal); + return false; + } + + + //------------------------------ Prepare variables for solution A & B + + // Get any crossbar point + Vector3f crossbar_pt; + auto& glist = fd.list_landmarks_goalposts; + if(goalNo == 1){ + crossbar_pt = glist.front().relPosCart; + }else{ + if(glist[0].absPos.x == glist[1].absPos.x){ + crossbar_pt = Vector3f::determineMidpoint( glist[0].relPosCart, glist[1].relPosCart); + }else{ + //extremely rare: there are other solutions when goalNo>2 but cost/benefit not worth it + crossbar_pt = glist.front().relPosCart; + } + } + + //------------------------------ Identify and apply solution A & B + + //Get line point closest to crossbar point + Vector3f p = l->linePointClosestToCartPoint( crossbar_pt ); + Vector3f possibleZvec = crossbar_pt - p; + float possibleZvecLength = possibleZvec.length(); + + if(fabsf(possibleZvecLength - 0.8) < 0.05){ //Solution A & B + Vector3f unit_zvec = possibleZvec / possibleZvecLength; + + // save as the new z axis orientation vector + prelimHeadToField.set(2,0,unit_zvec.x); + prelimHeadToField.set(2,1,unit_zvec.y); + prelimHeadToField.set(2,2,unit_zvec.z); + + find_z(unit_zvec); + return true; + } + + //------------------------------ Apply solution C + + Vector3f crossbar_left_vec, crossbar_midp; //this crossbar vector points left if seen from the midfield (this is important for the cross product) + + const auto& goal_mm = Field::list_8_landmarks::goal_mm; + const auto& goal_mp = Field::list_8_landmarks::goal_mp; + const auto& goal_pm = Field::list_8_landmarks::goal_pm; + const auto& goal_pp = Field::list_8_landmarks::goal_pp; + + if( goal_mm.visible && goal_mp.visible){ + crossbar_left_vec = goal_mm.relPosCart - goal_mp.relPosCart; + crossbar_midp = (goal_mm.relPosCart + goal_mp.relPosCart)/2; + }else if( goal_pp.visible && goal_pm.visible){ + crossbar_left_vec = goal_pp.relPosCart - goal_pm.relPosCart; + crossbar_midp = (goal_pp.relPosCart + goal_pm.relPosCart)/2; + } + + /** + * Check if angle between line and crossbar is between 45deg and 135deg + * 45deg < acos(line.crossbar / |line||crossbar|) < 135deg <=> + * | line.crossbar / |line||crossbar| | < cos(45deg) <=> + * | line.crossbar | < cos(45deg) * |line| * ~2.1 <=> + * | line.crossbar | < 1.485 * |line| + */ + Vector3f lvec = l->get_cart_vector(); + if( goalNo > 1 && fabsf(lvec.innerProduct(crossbar_left_vec)) < 1.485 * l->length ){ + Vector3f Zvec; + if(l->startc.dist(crossbar_midp) > l->endc.dist(crossbar_midp)){ + Zvec = lvec.crossProduct(crossbar_left_vec); + }else{ + Zvec = crossbar_left_vec.crossProduct(lvec); + } + Zvec = Zvec.normalize(); // get unit vector + + // save as the new z axis orientation vector + prelimHeadToField.set(2,0,Zvec.x); + prelimHeadToField.set(2,1,Zvec.y); + prelimHeadToField.set(2,2,Zvec.z); + + find_z(Zvec); + return true; + } + + + + stats_change_state(FAILz); + return false; //no solution was found +} + +/** + * Find the best fitting ground plane's normal vector using Singular Value Decomposition + * Also compute the agent's height based on the ground references' centroid + * Dependency: at least 3 ground references + */ +void LocalizerV2::fit_ground_plane(){ + + Field& fd = SField::getInstance(); + + const auto& ground_markers = fd.list_weighted_ground_markers; + const int ground_m_size = ground_markers.size(); + + //------------------------------------ Compute groundmarks plane (if we have at least 3 groundmarks) + + gsl_matrix *A, *V; + gsl_vector *S, *work; + + A = gsl_matrix_alloc(ground_m_size, 3); + V = gsl_matrix_alloc(3, 3); + S = gsl_vector_alloc(3); + work = gsl_vector_alloc(3); + + // Find the centroid + Vector3f centroid(0,0,0); + for(const auto& g : ground_markers){ // Insert all weighted groundmarks in matrix + centroid += g.relPosCart; + } + centroid /= (float)ground_m_size; + + // Insert all groundmarks in matrix after subtracting the centroid + for(int i=0; i 0 (replacing x=0, y=0, z=0) + * d < 0 + * + * However, if the agent is lying down, the optimized plane may be slightly above its head due to vision errors + * So, if we have a better reference point, such as a goal post, we use it + */ + + if(!fd.list_landmarks_goalposts.empty()){ //If there are visible goal posts + const Vector3f& aerialpt = fd.list_landmarks_goalposts.front().relPosCart; //random aerial point (goal post) + if( a*aerialpt.x + b*aerialpt.y + c*aerialpt.z < d ){ //the goalpost is on the negative side, so we invert the normal vector + a=-a; b=-b; c=-c; + } + }else{ //If there are no visible goal posts, we rely on the agent's head + if(d > 0){// the normal vectors points down, so we invert it (we ignore 'd' from this point forward) + a=-a; b=-b; c=-c; + } + } + + + // save the ground plane's normal vector as the new z axis orientation vector + prelimHeadToField.set(2,0,a); + prelimHeadToField.set(2,1,b); + prelimHeadToField.set(2,2,c); + + // compute the agent's height + float h = max( -centroid.x*a - centroid.y*b - centroid.z*c ,0.064); + prelimHeadToField.set(2,3, h ); + + //Set public independent coordinate z (Not a problem: may be out of sync with transf matrix) + last_z = final_z; + final_z = h; + _is_head_z_uptodate = true; + +} + +/** + * Compute translation in z (height) + * Note: Apparently there's no real benefit in involving goalposts (weighted or not), only when the + * visible objects are below 5/6, and even then the difference is minimal. + */ +void LocalizerV2::find_z(const Vector3f& Zvec){ + + Field& fd = SField::getInstance(); + + Vector3f zsum; + for(const auto& g: fd.list_weighted_ground_markers){ + zsum += g.relPosCart; + } + + //Minimum height: 0.064m + float z = max( -(zsum/fd.list_weighted_ground_markers.size()).innerProduct(Zvec) ,0.064f); + + prelimHeadToField.set( 2,3,z ); + + //Set public independent coordinate z (Not a problem: may be out of sync with transf matrix) + last_z = final_z; + final_z = z; + _is_head_z_uptodate = true; + +} + + +/** + * Computes mapping error using distance probabilities + * @return negative log of ["normalized" probability = (p1*p2*p3*...*pn)^(1/n)] + */ +double LocalizerV2::map_error_logprob(const gsl_vector *v, void *params){ + + float angle; + Field& fd = SField::getInstance(); + + //Get angle from optimization vector, or from params (as a constant) + if(v->size == 3){ + angle = gsl_vector_get(v,2); + }else{ + angle = *(float *)params; + } + + Matrix4D& transfMat = SLocalizerV2::getInstance().prelimHeadToField; + Vector3f Zvec(transfMat.get(2,0), transfMat.get(2,1), transfMat.get(2,2)); + + Vector3f Xvec, Yvec; + fast_compute_XYvec_from_Zvec(Zvec, angle, Xvec, Yvec ); + + //These are the transformation coefficients that are being optimized + transfMat.set(0,0,Xvec.x); + transfMat.set(0,1,Xvec.y); + transfMat.set(0,2,Xvec.z); + transfMat.set(0,3,gsl_vector_get(v, 0)); + transfMat.set(1,0,Yvec.x); + transfMat.set(1,1,Yvec.y); + transfMat.set(1,2,Yvec.z); + transfMat.set(1,3,gsl_vector_get(v, 1)); + + Matrix4D inverseTransMat = transfMat.inverse_tranformation_matrix(); + + + double total_logprob = 0; + int total_err_cnt =0; + + //Add log probability of unknown markers (with known corresponding field segment) + for(const auto& u : fd.list_unknown_markers){ + + + //We know the closest field segment, so we can bring it to the agent's frame + Vector3f rel_field_s_start = inverseTransMat * u.fieldSeg->point[0]->get_vector(); + Vector3f rel_field_s_end = inverseTransMat * u.fieldSeg->point[1]->get_vector(); + + Line6f rel_field_s(rel_field_s_start, rel_field_s_end, u.fieldSeg->length); //Convert to Line6f + + Vector3f closest_polar_pt = rel_field_s.segmentPointClosestToCartPoint(u.relPosCart).toPolar(); + + total_logprob += FieldNoise::log_prob_r(closest_polar_pt.x, u.relPosPolar.x); + total_logprob += FieldNoise::log_prob_h(closest_polar_pt.y, u.relPosPolar.y); + total_logprob += FieldNoise::log_prob_v(closest_polar_pt.z, u.relPosPolar.z); + total_err_cnt++; + + } + + //Add log probability of known markers + for(const auto& k : fd.list_known_markers){ + + //Bring marker to agent's frame + Vector3f rel_k = (inverseTransMat * k.absPos.get_vector()).toPolar(); + + total_logprob += FieldNoise::log_prob_r(rel_k.x, k.relPosPolar.x); + total_logprob += FieldNoise::log_prob_h(rel_k.y, k.relPosPolar.y); + total_logprob += FieldNoise::log_prob_v(rel_k.z, k.relPosPolar.z); + total_err_cnt++; + + } + + //return log of "normalized" probability = (p1*p2*p3*...*pn)^(1/n) + //negative because the optimization method minimizes the loss function + double logNormProb = -total_logprob / total_err_cnt; + + if(!gsl_finite(logNormProb)) return 1e6; //fix + + return logNormProb; + +} + + + +/** + * Computes mapping error using 2d euclidian distances + * @return average distance + */ +double LocalizerV2::map_error_2d(const gsl_vector *v, void *params){ + + float angle; + Field& fd = SField::getInstance(); + + //Get angle from optimization vector, or from params (as a constant) + if(v->size == 3){ + angle = gsl_vector_get(v,2); + }else{ + angle = *(float *)params; + } + + Matrix4D& transfMat = SLocalizerV2::getInstance().prelimHeadToField; + Vector3f Zvec(transfMat.get(2,0), transfMat.get(2,1), transfMat.get(2,2)); + + Vector3f Xvec, Yvec; + fast_compute_XYvec_from_Zvec(Zvec, angle, Xvec, Yvec ); + + + //These are the transformation coefficients that are being optimized + transfMat.set(0,0,Xvec.x); + transfMat.set(0,1,Xvec.y); + transfMat.set(0,2,Xvec.z); + transfMat.set(0,3,gsl_vector_get(v, 0)); + transfMat.set(1,0,Yvec.x); + transfMat.set(1,1,Yvec.y); + transfMat.set(1,2,Yvec.z); + transfMat.set(1,3,gsl_vector_get(v, 1)); + + + float total_err = 0; + int total_err_cnt =0; + for(const Line6f& l : fd.list_segments){ + + //Compute line absolute coordinates according to current transformation + Vector3f ls = transfMat * l.startc; + Vector3f le = transfMat * l.endc; + + //Compute line angle and establish a tolerance + float l_angle = 0; + float l_angle_tolerance = 10; //default full tolerance (no point in being larger than pi/2 but no harm either) + + if(l.length > 0.8){ + //This is the easy case: find the angle and establish a small tolerance (which allows some visual rotation attempts) + l_angle = atan2f(le.y - ls.y, le.x - ls.x); + if(l_angle < 0) { l_angle += 3.14159265f; } //this is a line, not a vector, so positive angles are enough + l_angle_tolerance = 0.35f; //20 degrees + + } else if(fd.list_segments.size() <= 3) { + //It gets to a point where the cost/benefit is not very inviting. If there are many lines (>3), + //the small ones are not as decisive for the mapping error. Otherwise, we proceed: + + //If the small line is touching a big line, they have different orientations (it's a characteristic from the field lines) + + for(const Line6f& lbig : fd.list_segments){ + if(lbig.length < 2 || &lbig == &l ) continue; //check if line is big and different from current + + if(lbig.segmentDistToSegment(l)<0.5){ + //this would only generate false positives with the halfway line and 4 ring lines (if enough vision error) + //but even then, their orientation would be very different, so the method still holds + + //---------- get angle perpendicular to bigline (that is either the small line's angle, or at least close enough) + + //get bigline angle + Vector3f lbigs = transfMat * lbig.startc; + Vector3f lbige = transfMat * lbig.endc; + l_angle = atan2f(lbige.y - lbigs.y, lbige.x - lbigs.x); + + // add 90deg while keeping the angle between 0-180deg (same logic used when l.length > 0.8) + if (l_angle < -1.57079632f){ l_angle += 4.71238898f; } //Q3 -> add pi*3/2 + else if(l_angle < 0 ){ l_angle += 1.57079632f; } //Q4 -> add pi/2 + else if(l_angle < 1.57079632f ){ l_angle += 1.57079632f; } //Q1 -> add pi/2 + else { l_angle -= 1.57079632f; } //Q2 -> subtract pi/2 + + //This large tolerance means that this small line can be matched with almost everything except perpendicular lines + l_angle_tolerance = 1.22f; //70 deg tolerance + break; //end search for close big lines + } + } + } + + + //this default error of 1e6f is applied when there is no match (which means the transf. matrix's Xvec/Yvec are wrong) + float min_err = 1e6f; + for(const auto& s : Field::cFieldLineSegments::list){ //find distance to closest field line + + //Skip field line if seen line is substantially larger + if( l.length > (s.length + 0.7) ){ continue; } + + //Skip field line if orientation does not match + float angle_difference = fabsf(l_angle - s.angle); + if(angle_difference > 1.57079632f) angle_difference = 3.14159265f - angle_difference; + if(angle_difference > l_angle_tolerance) continue; + + //Error is the sum of the distance of a single line segment to both endpoints of seen line + float err = Field::fieldLineSegmentDistToCart2DPoint(s,ls.to2d()); + if(err < min_err) err += Field::fieldLineSegmentDistToCart2DPoint(s,le.to2d()); + + if(err < min_err) min_err = err; + } + + total_err += min_err; + total_err_cnt+=2; //a line has 2 points, double the weight of a single landmark + + } + + + for(const Field::sMarker& m : fd.list_landmarks){ + + Vector3f lpt = transfMat * m.relPosCart; //compute absolute coordinates according to current transformation + + float err = lpt.to2d().getDistanceTo(Vector(m.absPos.x,m.absPos.y)); + total_err += err > 0.5 ? err * 100 : err; + total_err_cnt++; + + } + + double avg_error = total_err / total_err_cnt; + + if(!gsl_finite(avg_error)) return 1e6; //fix + + return avg_error; //return average error +} + + + + +/** + * Apply fine tuning directly on the prelimHeadToField matrix + * 1st - improve map fitting + * 2nd - identify line segments and their endpoints + * 3rd - fine tune again using known markers + * @param initial_angle initial angle of Xvec around Zvec + * @param initial_x initial translation in x + * @param initial_y initial translation in y + */ +bool LocalizerV2::fine_tune(float initial_angle, float initial_x, float initial_y){ + + Field& fd = SField::getInstance(); + + //Statistics before fine tune + counter_fineTune += stats_sample_position_error(Vector3f(initial_x,initial_y,prelimHeadToField.get(11)), world.my_cheat_abs_cart_pos, errorSum_fineTune_before); + + //Fine tune, changing the initial parameters directly + if(!fine_tune_aux(initial_angle, initial_x, initial_y, false)) return false; + + //Statistics for 1st fine tune + stats_sample_position_error(Vector3f(initial_x,initial_y,prelimHeadToField.get(11)), world.my_cheat_abs_cart_pos, errorSum_fineTune_euclidianDist); + + //Identify new markers + fd.update_from_transformation(prelimHeadToField); + + //Probabilistic fine tune + fine_tune_aux(initial_angle, initial_x, initial_y, true); + + //Statistics for 2nd fine tune + stats_sample_position_error(prelimHeadToField.toVector3f(), world.my_cheat_abs_cart_pos, errorSum_fineTune_probabilistic); + + //Update unknown markers absolute position based on refined transformation matrix + fd.update_unknown_markers(prelimHeadToField); + + return true; +} + + +/** + * Apply fine tuning: + * - directly on: initial_angle, initial_x, initial_y (if use_probabilities == false) + * - directly on the prelimHeadToField matrix using probabilities (if use_probabilities == true) + * @param initial_angle initial angle of Xvec around Zvec + * @param initial_x initial translation in x + * @param initial_y initial translation in y + */ +bool LocalizerV2::fine_tune_aux(float &initial_angle, float &initial_x, float &initial_y, bool use_probabilities){ + + int status, iter=0; + gsl_vector* x = create_gsl_vector<3>({initial_x, initial_y, initial_angle}); // Initial transformation + gsl_vector* ss = create_gsl_vector<3>({0.02, 0.02, 0.03}); // Set initial step sizes + gsl_multimin_function minex_func = {map_error_2d, 3, nullptr}; // error func, variables no., params + if(use_probabilities) minex_func.f = map_error_logprob; // probablity-based error function + + const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2; // algorithm type + gsl_multimin_fminimizer *s = gsl_multimin_fminimizer_alloc (T, 3); // allocate workspace + gsl_multimin_fminimizer_set (s, &minex_func, x, ss); // set workspace + + float best_x, best_y, best_ang; + + + do{ + iter++; + status = gsl_multimin_fminimizer_iterate(s); + + //*s holds the best solution, not the last solution + best_x = gsl_vector_get (s->x, 0); + best_y = gsl_vector_get (s->x, 1); + best_ang = gsl_vector_get (s->x, 2); + + if (status) break; + + double size = gsl_multimin_fminimizer_size (s); //minimizer-specific characteristic size + status = gsl_multimin_test_size (size, 1e-3); //This size can be used as a stopping criteria, as the simplex contracts itself near the minimum + + } + while ((status == GSL_CONTINUE || use_probabilities) && iter < 40); + + float best_map_error = s->fval; + + gsl_vector_free(x); + gsl_vector_free(ss); + gsl_multimin_fminimizer_free (s); + + if(!use_probabilities){ + if(best_map_error > 0.10){ + stats_change_state(FAILtune); + return false; + }else{ + initial_angle = best_ang; + initial_x = best_x; + initial_y = best_y; + return true; + } + } + + /** + * At this point, use_probabilities is true + * Note: The transformations are directly tested on prelimHeadToField but it currently + * holds the last test, so we set it manually here to the best found solution + */ + + //Convert angle into Xvec and Yvec + Vector3f Zvec(prelimHeadToField.get(2,0), prelimHeadToField.get(2,1), prelimHeadToField.get(2,2)); + Vector3f Xvec, Yvec; + fast_compute_XYvec_from_Zvec(Zvec, best_ang, Xvec, Yvec ); + + prelimHeadToField.set(0,0, Xvec.x); + prelimHeadToField.set(0,1, Xvec.y); + prelimHeadToField.set(0,2, Xvec.z); + prelimHeadToField.set(0,3, best_x); + prelimHeadToField.set(1,0, Yvec.x); + prelimHeadToField.set(1,1, Yvec.y); + prelimHeadToField.set(1,2, Yvec.z); + prelimHeadToField.set(1,3, best_y); + + return true; +} + +/** + * Find XY translation/rotation + * A unique solution is guaranteed if Zvec points in the right direction + * Requirement: 2 visible landmarks + */ +bool LocalizerV2::find_xy(){ + + Field& fd = SField::getInstance(); + + Vector3f Zvec(prelimHeadToField.get(2,0), prelimHeadToField.get(2,1), prelimHeadToField.get(2,2)); + + Field::sMarker *m1 = nullptr, *m2 = nullptr; + + //Get as many corners as possible + if(fd.list_landmarks_corners.size()>1){ + m1 = &fd.list_landmarks_corners[0]; + m2 = &fd.list_landmarks_corners[1]; + }else if(fd.list_landmarks_corners.size()==1){ + m1 = &fd.list_landmarks_corners[0]; + m2 = &fd.list_landmarks_goalposts[0]; + }else{ + m1 = &fd.list_landmarks_goalposts[0]; + m2 = &fd.list_landmarks_goalposts[1]; + } + + Vector3f realVec(m2->absPos.x - m1->absPos.x, m2->absPos.y - m1->absPos.y, m2->absPos.z - m1->absPos.z); + float real_angle = atan2f(realVec.y, realVec.x); //angle of real vector + + Vector3f seenVec(m2->relPosCart - m1->relPosCart); + Vector3f rotated_abs_vec = fast_rotate_around_ground_axis(seenVec, Zvec); + float seen_angle = atan2f(rotated_abs_vec.y, rotated_abs_vec.x); //angle of real vector + + float AgentAngle = real_angle - seen_angle; //no normalization is needed + + + Vector3f Xvec, Yvec; + fast_compute_XYvec_from_Zvec(Zvec, AgentAngle, Xvec, Yvec ); + + /** + * Let m be a landmark, rel:(mx,my,mz), abs:(mabsx, mabsy, mabsz) + * XvecX*mx + XvecY*my + XvecZ*mz + AgentX = mabsx + * AgentX = mabsx - (XvecX*mx + XvecY*my + XvecZ*mz) + * AgentX = mabsx - (XvecX . m) + * + * Generalizing for N estimates: + * AgentX = sum( mabsx - (XvecX . m) )/N + */ + float initial_x = 0, initial_y = 0; + for(const Field::sMarker& m : fd.list_landmarks){ + initial_x += m.absPos.x - Xvec.innerProduct(m.relPosCart); + initial_y += m.absPos.y - Yvec.innerProduct(m.relPosCart); + } + + initial_x /= fd.list_landmarks.size(); + initial_y /= fd.list_landmarks.size(); + + + return fine_tune(AgentAngle, initial_x, initial_y); + +} + +bool LocalizerV2::guess_xy(){ + Field& fd = SField::getInstance(); + + //Get Zvec from previous steps + Vector3f Zvec(prelimHeadToField.get(2,0), prelimHeadToField.get(2,1), prelimHeadToField.get(2,2)); + Vector last_known_position(head_position.x, head_position.y); + + //------------------------------------------------------------ Get longest line and use it as X or Y vector + + const Line6f* longestLine = &fd.list_segments.front(); + for(const Line6f& l : fd.list_segments){ + if(l.length > longestLine->length) longestLine = &l; + } + + if(longestLine->length < 1.6){ + stats_change_state(FAILguessLine); + return false; //largest line is too short, it could be mistaken for a ring line + } + + //Rotate line to real ground plane, where it loses the 3rd dimension + Vector3f longestLineVec = longestLine->endc - longestLine->startc; + Vector3f rotated_abs_line = fast_rotate_around_ground_axis(longestLineVec, Zvec); + + //The line can be aligned with X or Y, positively or negatively (these angles don't need to be normalized) + float fixed_angle[4]; + fixed_angle[0] = -atan2f(rotated_abs_line.y,rotated_abs_line.x); //if longestLineVec is Xvec + fixed_angle[1] = fixed_angle[0] + 3.14159265f; //if longestLineVec is -Xvec + fixed_angle[2] = fixed_angle[0] + 1.57079633f; //if longestLineVec is Yvec + fixed_angle[3] = fixed_angle[0] - 1.57079633f; //if longestLineVec is -Yvec + + //------------------------------------------------------------ Get initial translation + + //if we see 1 landmark, we use it, if not, we get the last position + + float initial_x[4], initial_y[4]; + bool noLandmarks = fd.list_landmarks.empty(); + + if(noLandmarks){ + + for(int i=0; i<4; i++){ + initial_x[i] = last_known_position.x; + initial_y[i] = last_known_position.y; + } + + } else { + + Vector3f Xvec = longestLineVec / longestLine->length; + Vector3f Yvec(Zvec.crossProduct(Xvec)); + + /** + * Let m be a landmark, rel:(mx,my,mz), abs:(mabsx, mabsy, mabsz) + * XvecX*mx + XvecY*my + XvecZ*mz + AgentX = mabsx + * AgentX = mabsx - (XvecX*mx + XvecY*my + XvecZ*mz) + * AgentX = mabsx - (XvecX . m) + */ + + const Field::sMarker& m = fd.list_landmarks.front(); + const float x_aux = Xvec.innerProduct(m.relPosCart); + const float y_aux = Yvec.innerProduct(m.relPosCart); + + initial_x[0] = m.absPos.x - x_aux; + initial_y[0] = m.absPos.y - y_aux; + initial_x[1] = m.absPos.x + x_aux; //2nd version: X is inverted + initial_y[1] = m.absPos.y + y_aux; //2nd version: Y is inverted + initial_x[2] = m.absPos.x + y_aux; //3rd version: X is inverted Y + initial_y[2] = m.absPos.y - x_aux; //3rd version: Y is X + initial_x[3] = m.absPos.x - y_aux; //4th version: X is Y + initial_y[3] = m.absPos.y + x_aux; //4th version: Y is inverted X + + } + + + //------------------------------------------------------------ Optimize XY rotation for each possible orientation + + + const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2; + gsl_multimin_fminimizer *s[4] = {nullptr,nullptr,nullptr,nullptr}; + gsl_vector *ss[4], *x[4]; + gsl_multimin_function minex_func[4]; + + size_t iter = 0; + int status; + double size; + + for(int i=0; i<4; i++){ + x[i] = create_gsl_vector<2>({initial_x[i], initial_y[i]}); // Initial transformation + ss[i] = create_gsl_vector<2>({1, 1}); //Set initial step sizes to 1 + + /* Initialize method */ + minex_func[i].n = 2; + minex_func[i].f = map_error_2d; + minex_func[i].params = &fixed_angle[i]; + + s[i] = gsl_multimin_fminimizer_alloc (T, 2); + gsl_multimin_fminimizer_set (s[i], &minex_func[i], x[i], ss[i]); + } + + /* start iterating */ + bool running[4] = {true,true,true,true}; + float current_error[4] = {1e6,1e6,1e6,1e6}; + float lowest_error = 1e6; + Vector best_xy[4]; + const int maximum_iterations = 50; + bool plausible_solution[4] = {false,false,false,false}; + do{ + iter++; + for(int i=0; i<4; i++){ + if(!running[i]) continue; + + status = gsl_multimin_fminimizer_iterate(s[i]); + + current_error[i] = s[i]->fval; + if(current_error[i] < lowest_error) lowest_error = current_error[i]; + + // Possible errors: + // GSL_ERROR ("incompatible size of x", GSL_EINVAL); This should only be a concern during code design + // GSL_ERROR ("contraction failed", GSL_EFAILED); Evaluation function produced non finite value + if (status) { + running[i]=false; //This is not a valid solution + continue; + } + + size = gsl_multimin_fminimizer_size (s[i]); //minimizer-specific characteristic size + status = gsl_multimin_test_size (size, 1e-2); //This size can be used as a stopping criteria, as the simplex contracts itself near the minimum + + if(status != GSL_CONTINUE || (lowest_error * 50 < current_error[i])) { //finished or aborted + best_xy[i].x = gsl_vector_get (s[i]->x, 0); + best_xy[i].y = gsl_vector_get (s[i]->x, 1); + running[i]=false; + plausible_solution[i]=(status == GSL_SUCCESS); //only valid if it converged to local minimum + continue; + } + + + } + + + } while (iter < maximum_iterations && (running[0] || running[1] || running[2] || running[3])); + + for(int i=0; i<4; i++){ + gsl_vector_free(x[i]); + gsl_vector_free(ss[i]); + gsl_multimin_fminimizer_free (s[i]); + } + + + //At this point, a solution is plausible if it converged to a local minimum + //So, we apply the remaining criteria for plausiblity + int plausible_count = 0; + int last_i; + for(int i=0; i<4; i++){ + if(!plausible_solution[i]) continue; + bool isDistanceOk = (!noLandmarks) || last_known_position.getDistanceTo(best_xy[i]) < 0.5; // distance to last known position + if(current_error[i] < 0.12 && isDistanceOk){ // mapping error + plausible_count++; + last_i = i; + } + } + + // If there is 1 landmark, and multiple options, the distance to last known pos is now used to eliminate candidates + if(!noLandmarks && plausible_count>1){ + plausible_count = 0; + for(int i=0; i<4; i++){ + if(plausible_solution[i] && last_known_position.getDistanceTo(best_xy[i]) < 0.5){ // distance to last known position + plausible_count++; + last_i = i; + } + } + } + + //Check if best solution is good if all others are not even plausible + if(plausible_count==0){ + stats_change_state(FAILguessNone); + return false; + }else if(plausible_count>1){ + stats_change_state(FAILguessMany); + return false; + }else if(current_error[last_i] > 0.06 || (noLandmarks && last_known_position.getDistanceTo(best_xy[last_i]) > 0.3)){ // mapping error / distance to last known position + stats_change_state(FAILguessTest); + return false; + } + + return fine_tune(fixed_angle[last_i],best_xy[last_i].x, best_xy[last_i].y); + +} + + + +/** + * Called to update every public variable (rotation + translation) + */ +void LocalizerV2::commit_everything(){ + + final_headTofieldTransform = prelimHeadToField; //Full transformation (relative to absolute) + + final_headTofieldTransform.inverse_tranformation_matrix( final_fieldToheadTransform ); //Full transformation (absolute to relative) + + for(int i=0; i<3; i++){ + + //Rotation (relative to absolute) + final_headTofieldRotate.set(i ,final_headTofieldTransform.get(i )); //Copy rotation line 1 + final_headTofieldRotate.set(i+4,final_headTofieldTransform.get(i+4)); //Copy rotation line 2 + final_headTofieldRotate.set(i+8,final_headTofieldTransform.get(i+8)); //Copy rotation line 3 + + //Rotation (absolute to relative) + final_fieldToheadRotate.set(i ,final_fieldToheadTransform.get(i )); //Copy rotation line 1 + final_fieldToheadRotate.set(i+4,final_fieldToheadTransform.get(i+4)); //Copy rotation line 2 + final_fieldToheadRotate.set(i+8,final_fieldToheadTransform.get(i+8)); //Copy rotation line 3 + } + + final_translation = final_headTofieldTransform.toVector3f(); + + _is_uptodate = true; + + _steps_since_last_update = 0; + + //Add current 3D position to history + position_history[position_history_ptr++] = final_translation; + if(position_history_ptr >= position_history.size()) position_history_ptr=0; + +} + + +Vector3f LocalizerV2::relativeToAbsoluteCoordinates(const Vector3f relativeCoordinates) const{ + return headTofieldTransform * relativeCoordinates; +} + +Vector3f LocalizerV2::absoluteToRelativeCoordinates(const Vector3f absoluteCoordinates) const{ + return fieldToheadTransform * absoluteCoordinates; +} + + + +/** + * Reset preliminary matrix with a constant for the first 3 rows + */ +void LocalizerV2::prelim_reset(){ + //Since it was initialized as identity matrix we never need to change the last row + for(int i=0; i<12; i++){ + prelimHeadToField.set(i,9999); //flag unchanged cells with a constant + } +} + +//================================================================================================= +//=============================================================================== useful statistics +//================================================================================================= + +Vector3f LocalizerV2::get_velocity(unsigned int n) const{ + //assert(n > 0 && n < position_history.size() && "LocalizerV2::get_velocity(unsigned int n) -> n must be between 1 and 9!"); + + int l = position_history.size() - 1; + + Vector3f current_pos = position_history[(position_history_ptr + l) % position_history.size()]; + Vector3f last_pos = position_history[(position_history_ptr + l - n) % position_history.size()]; + + return current_pos - last_pos; +} + +//================================================================================================= +//================================================================================ debug statistics +//================================================================================================= + + +void LocalizerV2::print_report() const{ + + if(counter_fineTune == 0){ + cout << "LocalizerV2 Report - Check if the server is providing cheat data.\n"; + return; + } + + if(counter_fineTune < 2) return; //otherwise, c-1=0 + const int &c = counter_fineTune; + const int &cb = counter_ball; + const int c1 = c-1; + const int cb1 = cb-1; + + const double* ptr = errorSum_fineTune_before; + float e1_2d_var = (ptr[4] - (ptr[3]*ptr[3]) / c) / c1; + float e1_3d_var = (ptr[6] - (ptr[5]*ptr[5]) / c) / c1; + float e1[] = { ptr[3]/c, sqrt(e1_2d_var), ptr[5]/c, sqrt(e1_3d_var), ptr[0]/c, ptr[1]/c, ptr[2]/c }; + + ptr = errorSum_fineTune_euclidianDist; + float e2_2d_var = (ptr[4] - (ptr[3]*ptr[3]) / c) / c1; + float e2_3d_var = (ptr[6] - (ptr[5]*ptr[5]) / c) / c1; + float e2[] = { ptr[3]/c, sqrt(e2_2d_var), ptr[5]/c, sqrt(e2_3d_var), ptr[0]/c, ptr[1]/c, ptr[2]/c }; + + ptr = errorSum_fineTune_probabilistic; + float e3_2d_var = (ptr[4] - (ptr[3]*ptr[3]) / c) / c1; + float e3_3d_var = (ptr[6] - (ptr[5]*ptr[5]) / c) / c1; + float e3[] = { ptr[3]/c, sqrt(e3_2d_var), ptr[5]/c, sqrt(e3_3d_var), ptr[0]/c, ptr[1]/c, ptr[2]/c }; + + ptr = errorSum_ball; + float e4_2d_var=0, e4_3d_var=0; + if(cb1 > 0){ + e4_2d_var = (ptr[4] - (ptr[3]*ptr[3]) / cb) / cb1; + e4_3d_var = (ptr[6] - (ptr[5]*ptr[5]) / cb) / cb1; + } + float e4[] = { ptr[3]/cb, sqrt(e4_2d_var), ptr[5]/cb, sqrt(e4_3d_var), ptr[0]/cb, ptr[1]/cb, ptr[2]/cb }; + + const int* st = state_counter; + printf("---------------------------------- LocalizerV2 Report ----------------------------------\n"); + printf("SAMPLING STAGE 2D-MAE 2D-STD 3D-MAE 3D-STD x-MBE y-MBE z-MBE\n"); + printf("Before fine-tune: %.4f %.4f %.4f %.4f %7.4f %7.4f %7.4f\n", e1[0],e1[1],e1[2],e1[3],e1[4],e1[5],e1[6]); + printf("After Euclidian dist. fit: %.4f %.4f %.4f %.4f %7.4f %7.4f %7.4f\n", e2[0],e2[1],e2[2],e2[3],e2[4],e2[5],e2[6]); + printf("After probabilistic fit: %.4f %.4f %.4f %.4f %7.4f %7.4f %7.4f\n", e3[0],e3[1],e3[2],e3[3],e3[4],e3[5],e3[6]); + printf("Ball: %.4f %.4f %.4f %.4f %7.4f %7.4f %7.4f\n\n", e4[0],e4[1],e4[2],e4[3],e4[4],e4[5],e4[6]); + printf("* MBE(Mean Bias Error) MAE(Mean Abs Error) STD(Standard Deviation)\n"); + printf("* Note: the cheat positions should be active in server (preferably with >2 decimal places)\n\n"); + printf("------------------LocalizerV2::run calls analysis:\n"); + printf("- Total: %i \n", st[RUNNING]); + printf("- Successful: %i \n", st[DONE]); + printf("- Blind agent: %i \n", st[BLIND]); + printf("- Almost blind: %i \n", st[MINFAIL] + st[FAILzNOgoal] + st[FAILzLine] + st[FAILz]); + printf("- Guess location fail: %i \n", st[FAILguessLine] + st[FAILguessNone] + st[FAILguessMany] + st[FAILguessTest]); + printf("--- Lines too short: %i \n", st[FAILguessLine]); + printf("--- No solution: %i \n", st[FAILguessNone]); + printf("--- >1 solution: %i \n", st[FAILguessMany]); + printf("--- Weak solution: %i \n", st[FAILguessTest]); + printf("- Eucl. tune fail: %i \n", st[FAILtune]); //Euclidian distance tune error above 6cm + printf("----------------------------------------------------------------------------------------\n"); + +} + +void LocalizerV2::stats_reset(){ + + counter_fineTune = 0; + for(int i=0; i= 3 noncollinear ground references (z=0) + * ASSUMPTION: the ground references are not collinear. Why? + * If we see 2 lines their endpoints are never collinear. + * If we see one and we are on top of it, the feet contact points can cause collinearity but it is very rare. + * SOLUTION: Find the best fitting ground plane's normal vector using Singular Value Decomposition + * + * 1.2. there are < 3 noncollinear ground references (z=0) + * + * Possible combinations: + * If there is 1 corner flag, either we have >= 3 ground references, or it is impossible. + * So, below, we assume there are 0 corner flags. + * + * | 0 lines + 0/1/2 feet | 1 line + 0 feet | + * -------------|------------------------|------------------| + * 0 goalposts | ----- | ----- | (Only 1 line: there is no way of identifying it) + * 1 goalpost | ----- | A,C | (1 goalpost and 0/1/2 feet: infinite solutions) + * 2 goalposts | * | B,C | + * + * + * If it sees 1 or 2 goalposts and only 1 line, we assume for A & B that it is the endline (aka goal line) + * + * SOLUTIONS: + * 1.2.A. IF IT IS THE GOALLINE. Find the line's nearest point (p) to the goalpost (g), Zvec = (g-p) / |Zvec| + * 1.2.B. IF IT IS THE GOALLINE. Find the line's nearest point (p) to the goalposts (g1,g2) midpoint (m), Zvec = (m-p) / |Zvec| + * (This solution is more accurate than using only 1 goalpost. Even m is more accurate, on average, than g1 or g2.) + * 1.2.C. IF IT IS NOT THE GOALLINE. There are 3 options: + * I - There are 2 goalposts (crossbar line) and an orthogonal line: + * Zvec = crossbar x line (or) line x crossbar (depending of the direction of both vectors) + * II - Other situation if the z translation coordinate was externally provided through machine learning: + * Find any horizontal line (e.g. line between 2 goalposts, or ground line) + * Let M be any mark with known absolute z, and let Z be the externally provided z coordinate: + * Find Zvec such that (HorLine.Zvec=0) and (Zvec.Mrel=Mabsz-Z) + * III - If z was not provided: + * Skip to last step. + * 1.2.*. This scenario was tested and it is not valid. In certain body positions there are two solutions, and even though + * one is correct and generally yields lower error, the other one is a local optimum outside the field. One could + * exclude the out-of-field solution with some mildly expensive modifications to the optimization's error function, + * but the out-of-field scenario is not unrealistic, so this is not the way. Adding an external z source could help + * increasing the error of the wrong local optimum, but it may not be enough. Another shortcoming of this scenario is + * when we see the goalposts from the opposite goal, creating large visual error. + * + * 1.3. impossible / not implemented: in this case skip to last step + * + * ----------------------------------------------------------------------------------- + * 2. Compute z: + * + * Here's what we know about the transformation matrix so far: + * | - | - | - | - | + * | - | - | - | - | + * | zx | zy | zz | ? | We want to know the translation in z + * | 0 | 0 | 0 | 1 | + * + * Given a random point (p) with known relative coordinates and known absolute z coordinate, + * we can find the translation in z: + * p.relx * zx + p.rely * zy + p.relz * zz + ? = p.absz + * + * If we do this for every point, we can then average z + * + * ----------------------------------------------------------------------------------- + * 3. Compute a rough estimate for entire transformation (2 first rows): + * + * Solution quality for possible combinations: + * + * short line (length < 0.5m) *hard to detect orientation, *generated displacement error is insuficient for optimization + * long line (length >= 0.5m) + * + * | 0 landmarks | 1 goalpost | 1 corner | >= 2 landmarks | + * -----------------|---------------|--------------|------------|----------------| + * 0 long lines | --- | --- | --- | A | + * 1 long line | --- | B+ | B | A | + * 2 long lines | B | B+ | B++ | A | + * + * SOLUTIONS: + * A - the solution is unique + * STEPS: + * - Compute the X-axis and Y-axis orientation from 2 landmarks + * - Average the translation for every visible landmark + * - Fine-tune XY translation/rotation + * B - there is more than 1 solution, so we use the last known position as reference + * Minimum Requirements: + * - longest line must be >1.6m so that we can extract the orientation (hor/ver) while not being mistaken for a ring line + * - there is exactly 1 plausible solution + * Notes: + * B+ (the solution is rarely unique) + * B++ (the solution is virtually unique but cannot be computed with the algorithm for A scenarios) + * STEPS: + * - Find 4 possible orientations based on the longest line (which should be either aligned with X or Y) + * - Determine reasonable initial translation: + * - If the agent sees 1 landmark: compute XY translation for each of the 4 orientations based on that 1 landmark + * - If the agent sees 0 landmarks: use last known position + * Note: Why not use always last known position if that is a criterion in the end? + * Because in this stage it would only delay the optimization. + * - Optimize the X/Y translation for every possible orientation + * - Perform quality assessment + * Plausible solution if: + * - Optimization converged to local minimum + * - Distance to last known position <50cm (applicable if no visible landmarks) + * - Mapping error <0.12m/point + * - Given the agent's FOV, inverse mapping error <0.2m/point (disabled in V2) + * NOTE: If there is 1 landmark, plausibility is defined by mapping errors, not distance to last known pos. So if + * one guess has the only acceptable mapping error, but is the farthest from previous position, it is still chosen. + * However, if >1 guess has acceptable mapping error, the 0.5m threshold is used to eliminate candidates. + * Likely if: + * - Plausible + * - Distance to last known position <30cm (applicable if no visible landmarks) + * - Mapping error <0.06m/point + * - Given the agent's FOV, inverse mapping error <0.1m/point (not currently active) + * - Choose likely solution if all others are not even plausible + * - Fine-tune XY translation/rotation + * + * ----------------------------------------------------------------------------------- + * 4. Identify visible elements and perform 2nd fine tune based on distance probabilites + * + * ----------------------------------------------------------------------------------- + * Last step. Analyze preliminary transformation matrix to update final matrices + * + * For the reasons stated in the beginning (see warning), if the preliminary matrix was not entirely set, the + * actual transformation matrix will not be changed. But the head position will always reflect the latest changes. + * + * + * =================================================================================== + * LOCALIZATION BASED ON PROBABILITY DENSITIES + * =================================================================================== + * ================================PROBABILITY DENSITY================================ + * + * For 1 distance measurement from RCSSSERVER3D: + * + * Error E = d/100 * A~N(0,0.0965^2) + B~U(-0.005,0.005) + * PDF[d/100 * A](w) = PDF[N(0,(d/100 * 0.0965)^2)](w) + * PDF[E](w) = PDF[N(0,(d/100 * 0.0965)^2)](w) convoluted with PDF[U(-0.005,0.005)](w) + * + * where d is the distance from a given [p]oint (px,py,pz) to the [o]bject (ox,oy,oz) + * and w is the [m]easurement error: w = d-measurement = sqrt((px-ox)^2+(py-oy)^2+(pz-oz)^2) - measurement + * + * PDF[E](w) -> PDF[E](p,o,m) + * --------------------------------------------------------------- + * For n independent measurements: + * + * PDF[En](p,on,mn) = PDF[E1](p,o1,m1) * PDF[E2](p,o2,m2) * PDF[E3](p,o3,m3) * ... + * --------------------------------------------------------------- + * Adding z estimation: + * + * PDF[zE](wz) = PDF[N(mean,std^2)](wz) + * where wz is the zError = estz - pz + * + * PDF[zE](wz) -> PDF[zE](pz,estz) + * PDF[En](p,on,mn,estz) = PDF[En](p,on,mn) * PDF[zE](pz,estz) + * =================================================================================== + * =====================================GRADIENT====================================== + * + * Grad(PDF[En](p,on,mn,estz)) wrt p = Grad( PDF[E1](p,o1,m1) * ... * PDF[E2](p,on,mn) * PDF[zE](pz,estz)) wrt {px,py,pz} + * + * Generalizing the product rule for n factors, we have: + * + * Grad(PDF[En](p,on,mn)) wrt p = sum(gradPDF[Ei] / PDF[Ei]) * prod(PDF[Ei]) + * Grad(PDF[En](p,on,mn)) wrt p = sum(gradPDF[Ei] / PDF[Ei]) * PDF[En](p,on,mn) + * + * note that: gradPDF[zE](pz,estz) wrt {px,py,pz} = {0,0,d/d_pz} + * =================================================================================== + * */ + +#pragma once +#include "Singleton.h" +#include "Field.h" +#include "Matrix4D.h" +#include "FieldNoise.h" + +#include //Linear least-squares fitting +#include //Singular value decomposition +#include //Multidimensional minimization + +class LocalizerV2 { + friend class Singleton; + +public: + + /** + * Compute 3D position and 3D orientation + * sets "is_uptodate" to true if there is new information available (rotation+translation) + * If no new information is available, the last known position is provided + */ + void run(); + + /** + * Print report (average errors + solution analysis) + */ + void print_report() const; + + /** + * Transformation matrices + * They are initialized as 4x4 identity matrices + */ + const Matrix4D &headTofieldTransform = final_headTofieldTransform; // rotation + translation + const Matrix4D &headTofieldRotate = final_headTofieldRotate; // rotation + const Matrix4D &fieldToheadTransform = final_fieldToheadTransform; // rotation + translation + const Matrix4D &fieldToheadRotate = final_fieldToheadRotate; // rotation + + /** + * Head position + * translation part of headTofieldTransform + */ + const Vector3f &head_position = final_translation; + + /** + * True if head_position and the transformation matrices are up to date + * (false if this is not a visual step, or not enough elements are visible) + */ + const bool &is_uptodate = _is_uptodate; + + /** + * Number of simulation steps since last update (see is_uptodate) + * If (is_uptodate==true) then "steps_since_last_update" is zero + */ + const unsigned int &steps_since_last_update = _steps_since_last_update; + + /** + * Head z coordinate + * This variable is often equivalent to head_position.z, but sometimes it differs. + * There are situations in which the rotation and translation cannot be computed, + * but the z-coordinate can still be found through vision. + * It should be used in applications which rely on z as an independent coordinate, + * such as detecting if the robot has fallen, or as machine learning observations. + * It should not be used for 3D transformations. + */ + const float &head_z = final_z; + + /** + * Since head_z can be computed in situations where self-location is impossible, + * this variable is set to True when head_z is up to date + */ + const bool &is_head_z_uptodate = _is_head_z_uptodate; + + /** + * Transform relative to absolute coordinates using headTofieldTransform + * @return absolute coordinates + */ + Vector3f relativeToAbsoluteCoordinates(const Vector3f relativeCoordinates) const; + + /** + * Transform absolute to relative coordinates using fieldToheadTransform + * @return relative coordinates + */ + Vector3f absoluteToRelativeCoordinates(const Vector3f absoluteCoordinates) const; + + /** + * Get 3D velocity (based on last n 3D positions) + * @param n number of last positions to evaluate (min 1, max 9) + * Example for n=3: + * current position: p0 (current time step) + * last position: p1 (typically* 3 time steps ago) + * position before: p2 (typically* 6 time steps ago) + * position before: p3 (typically* 9 time steps ago) + * RETURN value: p0-p3 + * *Note: number of actual time steps depends on server configuration and whether + * the agent was able to self-locate on the last n visual steps + * @return 3D velocity vector + */ + Vector3f get_velocity(unsigned int n) const; + + /** + * Get last known head z coordinate + * Note: this variable is based on head_z. It can be used as an independent coordinate, + * but it should not be used for 3D transformations, as it may be out of sync with + * the x and y coordinates. (see head_z) + * @return last known head z coordinate + */ + float get_last_head_z() const {return last_z;} + + +private: + + //================================================================================================= + //============================================================================ main private methods + //================================================================================================= + + bool find_z_axis_orient_vec(); //returns true if successful + void fit_ground_plane(); + + void find_z(const Vector3f& Zvec); + bool find_xy(); + bool guess_xy(); + + bool fine_tune_aux(float &initial_angle, float &initial_x, float &initial_y, bool use_probabilities); + bool fine_tune(float initial_angle, float initial_x, float initial_y); + + static double map_error_logprob(const gsl_vector *v, void *params); + static double map_error_2d(const gsl_vector *v, void *params); + + void commit_everything(); + + + //================================================================================================= + //=================================================================== private transformation matrix + //================================================================================================= + + //PRELIMINARY MATRIX - where all operations are stored + //if the algorithm is not successful, the final matrix is not modified + void prelim_reset(); + Matrix4D prelimHeadToField = Matrix4D(); //initialized as identity matrix + + //FINAL MATRIX - the user has access to a public const reference of the variables below + Matrix4D final_headTofieldTransform; // rotation + translation + Matrix4D final_headTofieldRotate; // rotation + Matrix4D final_fieldToheadTransform; // rotation + translation + Matrix4D final_fieldToheadRotate; // rotation + + Vector3f final_translation; //translation + + float final_z; //independent z translation (may be updated more often) + + //================================================================================================= + //=============================================================================== useful statistics + //================================================================================================= + + std::array position_history; + unsigned int position_history_ptr = 0; + + float last_z = 0.5; + + unsigned int _steps_since_last_update = 0; + bool _is_uptodate = false; + bool _is_head_z_uptodate = false; + + //================================================================================================= + //================================================================================ debug statistics + //================================================================================================= + + int stats_sample_position_error(const Vector3f sample, const Vector3f& cheat, double error_placeholder[]); + void stats_reset(); + + double errorSum_fineTune_before[7] = {0}; //[0,1,2]- xyz err sum, [3]-2D err sum, [4]-2D err sq sum, [5]-3D err sum, [6]-3D err sq sum + double errorSum_fineTune_euclidianDist[7] = {0}; //[0,1,2]- xyz err sum, [3]-2D err sum, [4]-2D err sq sum, [5]-3D err sum, [6]-3D err sq sum + double errorSum_fineTune_probabilistic[7] = {0}; //[0,1,2]- xyz err sum, [3]-2D err sum, [4]-2D err sq sum, [5]-3D err sum, [6]-3D err sq sum + double errorSum_ball[7] = {0}; //[0,1,2]- xyz err sum, [3]-2D err sum, [4]-2D err sq sum, [5]-3D err sum, [6]-3D err sq sum + int counter_fineTune = 0; + int counter_ball = 0; + + enum STATE{NONE, RUNNING, MINFAIL, BLIND, FAILzNOgoal, FAILzLine, FAILz, FAILtune, FAILguessLine, FAILguessNone, FAILguessMany, FAILguessTest, DONE, ENUMSIZE}; + STATE state = NONE; + + void stats_change_state(enum STATE s); + int state_counter[STATE::ENUMSIZE] = {0}; + +}; + + +typedef Singleton SLocalizerV2; \ No newline at end of file diff --git a/cpp/localization/Makefile b/cpp/localization/Makefile new file mode 100644 index 0000000..d37469f --- /dev/null +++ b/cpp/localization/Makefile @@ -0,0 +1,15 @@ +src = $(wildcard *.cpp) +obj = $(src:.c=.o) + +LDFLAGS = -lgsl -lgslcblas +CFLAGS = -O3 -shared -std=c++11 -fPIC -Wall $(PYBIND_INCLUDES) + +all: $(obj) + g++ $(CFLAGS) -o localization.so $^ $(LDFLAGS) + +debug: $(filter-out lib_main.cpp,$(obj)) + g++ -O0 -std=c++14 -Wall -g -o debug.bin debug_main.cc $^ $(LDFLAGS) + +.PHONY: clean +clean: + rm -f $(obj) all diff --git a/cpp/localization/Matrix4D.cpp b/cpp/localization/Matrix4D.cpp new file mode 100644 index 0000000..217d08e --- /dev/null +++ b/cpp/localization/Matrix4D.cpp @@ -0,0 +1,303 @@ +#include "Matrix4D.h" + + + +Matrix4D::Matrix4D() { + // identity matrix + const float tmp[M_LENGTH] = { + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1 + }; + + for (int i = 0; i < M_LENGTH; i++) + content[i] = tmp[i]; +} + +Matrix4D::Matrix4D(const float a[M_LENGTH]) { + // creates a matrix using a vector of floats + for (int i = 0; i < M_LENGTH; i++) + content[i] = a[i]; +} + +Matrix4D::Matrix4D(const Matrix4D& other) { + // creates a matrix using another matrix + for (int i = 0; i < M_LENGTH; i++) + content[i] = other.content[i]; +} + +Matrix4D::Matrix4D(const Vector3f& v) { + float x = v.getX(); + float y = v.getY(); + float z = v.getZ(); + + // gets a translation matrix from xyz coordinates + const float tmp[M_LENGTH] = { + 1, 0, 0, x, + 0, 1, 0, y, + 0, 0, 1, z, + 0, 0, 0, 1 + }; + + for (int i = 0; i < M_LENGTH; i++) + content[i] = tmp[i]; +} + +Matrix4D::~Matrix4D() { + // nothing to do +} + +void Matrix4D::set(unsigned i, float value) { + content[i] = value; +} + +void Matrix4D::set(unsigned i, unsigned j, float value) { + content[M_COLS * i + j] = value; +} + +float Matrix4D::get(unsigned i) const{ + return content[i]; +} + +float Matrix4D::get(unsigned i, unsigned j) const{ + return content[M_COLS * i + j]; +} + +Matrix4D Matrix4D::operator+(const Matrix4D& other) const { + // sums two matrices + float tmp[M_LENGTH]; + + for (int i = 0; i < M_LENGTH; i++) + tmp[i] = this->content[i] + other.content[i]; + + return Matrix4D(tmp); +} + +Matrix4D Matrix4D::operator-(const Matrix4D& other) const { + // subtracts a matrix from another + float tmp[M_LENGTH]; + + for (int i = 0; i < M_LENGTH; i++) + tmp[i] = this->content[i] - other.content[i]; + + return Matrix4D(tmp); +} + +Matrix4D Matrix4D::operator*(const Matrix4D& other) const { + + // multiplies two matrices + float tmp[M_LENGTH]; + + for (int i = 0; i < M_ROWS; i++) { + for (int j = 0; j < M_COLS; j++) { + tmp[M_COLS * i + j] = 0; + + for (int k = 0; k < M_COLS; k++) + tmp[M_COLS * i + j] += this->content[M_COLS * i + k] * other.content[M_COLS * k + j]; + } + } + + return Matrix4D(tmp); +} + +Vector3f Matrix4D::operator*(const Vector3f& vec) const { + // multiplies this matrix by a vector of four floats + // the first three are from vec and the remaining float is 1.0 + float x = 0; + float y = 0; + float z = 0; + + x = this->content[0] * vec.getX(); + x += this->content[1] * vec.getY(); + x += this->content[2] * vec.getZ(); + x += this->content[3]; + + y = this->content[4] * vec.getX(); + y += this->content[5] * vec.getY(); + y += this->content[6] * vec.getZ(); + y += this->content[7]; + + z = this->content[8] * vec.getX(); + z += this->content[9] * vec.getY(); + z += this->content[10] * vec.getZ(); + z += this->content[11]; + + return Vector3f(x, y, z); +} + +void Matrix4D::operator=(const Matrix4D& other) { + // assigns another matrix to this one + for (int i = 0; i < M_LENGTH; i++) + content[i] = other.content[i]; +} + +bool Matrix4D::operator==(const Matrix4D& other) const { + // checks whether this matrix is equal to another + for (int i = 0; i < M_LENGTH; i++) + if (content[i] != other.content[i]) + return false; + + return true; +} + +void Matrix4D::operator+=(const Matrix4D& other) { + // sums this matrix to another and returns the result + for (int i = 0; i < M_LENGTH; i++) + content[i] += other.content[i]; +} + +void Matrix4D::operator-=(const Matrix4D& other) { + // subtracts this matrix from another and returns the result + for (int i = 0; i < M_LENGTH; i++) + content[i] -= other.content[i]; +} + +float& Matrix4D::operator[](const unsigned pos) { + // gets a value from position + return content[pos]; +} + +Vector3f Matrix4D::toVector3f() const { + // gets the translation vector from the matrix + float x = get(0, M_COLS - 1); + float y = get(1, M_COLS - 1); + float z = get(2, M_COLS - 1); + + return Vector3f(x, y, z); +} + +Matrix4D Matrix4D::transpose() { + // returns the transpose of this matrix + Matrix4D result; + + for (int i = 0; i < M_ROWS; i++) + for (int j = 0; j < M_COLS; j++) + result.set(j, i, get(i, j)); + + return result; +} + +Matrix4D Matrix4D::inverse_tranformation_matrix() const { + + Matrix4D inv; //Initialized as identity matrix + inverse_tranformation_matrix(inv); + return inv; +} + +void Matrix4D::inverse_tranformation_matrix(Matrix4D& inv) const { + + //Rotation + inv[0] = content[0]; inv[1] = content[4]; inv[2] = content[8]; + inv[4] = content[1]; inv[5] = content[5]; inv[6] = content[9]; + inv[8] = content[2]; inv[9] = content[6]; inv[10] = content[10]; + + //Translation + inv[3] = -content[0]*content[3] - content[4]*content[7] - content[8]*content[11]; + inv[7] = -content[1]*content[3] - content[5]*content[7] - content[9]*content[11]; + inv[11] = -content[2]*content[3] - content[6]*content[7] - content[10]*content[11]; + +} + +bool Matrix4D::inverse(Matrix4D& inverse_out) const{ + // returns the inverse of this matrix + + float inv[16], det; + const float* m = content; + int i; + + inv[0] = m[5] * m[10] * m[15] - m[5] * m[11] * m[14] - m[9] * m[6] * m[15] + m[9] * m[7] * m[14] + m[13] * m[6] * m[11] - m[13] * m[7] * m[10]; + inv[4] = -m[4] * m[10] * m[15] + m[4] * m[11] * m[14] + m[8] * m[6] * m[15] - m[8] * m[7] * m[14] - m[12] * m[6] * m[11] + m[12] * m[7] * m[10]; + inv[8] = m[4] * m[9] * m[15] - m[4] * m[11] * m[13] - m[8] * m[5] * m[15] + m[8] * m[7] * m[13] + m[12] * m[5] * m[11] - m[12] * m[7] * m[9]; + inv[12] = -m[4] * m[9] * m[14] + m[4] * m[10] * m[13] + m[8] * m[5] * m[14] - m[8] * m[6] * m[13] - m[12] * m[5] * m[10] + m[12] * m[6] * m[9]; + inv[1] = -m[1] * m[10] * m[15] + m[1] * m[11] * m[14] + m[9] * m[2] * m[15] - m[9] * m[3] * m[14] - m[13] * m[2] * m[11] + m[13] * m[3] * m[10]; + inv[5] = m[0] * m[10] * m[15] - m[0] * m[11] * m[14] - m[8] * m[2] * m[15] + m[8] * m[3] * m[14] + m[12] * m[2] * m[11] - m[12] * m[3] * m[10]; + inv[9] = -m[0] * m[9] * m[15] + m[0] * m[11] * m[13] + m[8] * m[1] * m[15] - m[8] * m[3] * m[13] - m[12] * m[1] * m[11] + m[12] * m[3] * m[9]; + inv[13] = m[0] * m[9] * m[14] - m[0] * m[10] * m[13] - m[8] * m[1] * m[14] + m[8] * m[2] * m[13] + m[12] * m[1] * m[10] - m[12] * m[2] * m[9]; + inv[2] = m[1] * m[6] * m[15] - m[1] * m[7] * m[14] - m[5] * m[2] * m[15] + m[5] * m[3] * m[14] + m[13] * m[2] * m[7] - m[13] * m[3] * m[6]; + inv[6] = -m[0] * m[6] * m[15] + m[0] * m[7] * m[14] + m[4] * m[2] * m[15] - m[4] * m[3] * m[14] - m[12] * m[2] * m[7] + m[12] * m[3] * m[6]; + inv[10] = m[0] * m[5] * m[15] - m[0] * m[7] * m[13] - m[4] * m[1] * m[15] + m[4] * m[3] * m[13] + m[12] * m[1] * m[7] - m[12] * m[3] * m[5]; + inv[14] = -m[0] * m[5] * m[14] + m[0] * m[6] * m[13] + m[4] * m[1] * m[14] - m[4] * m[2] * m[13] - m[12] * m[1] * m[6] + m[12] * m[2] * m[5]; + inv[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] - m[9] * m[2] * m[7] + m[9] * m[3] * m[6]; + inv[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] + m[8] * m[2] * m[7] - m[8] * m[3] * m[6]; + inv[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] - m[8] * m[1] * m[7] + m[8] * m[3] * m[5]; + inv[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] + m[8] * m[1] * m[6] - m[8] * m[2] * m[5]; + + det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12]; + + if (det == 0) + return false; + + det = 1.0 / det; + + for (i = 0; i < 16; i++) + inverse_out.set(i, inv[i] * det); + + return true; +} + + +Matrix4D Matrix4D::rotationX(float angle) { + // rotates this matrix around the x-axis + const float tmp[M_LENGTH] = { + 1, 0, 0, 0, + 0, Cos(angle), -Sin(angle), 0, + 0, Sin(angle), Cos(angle), 0, + 0, 0, 0, 1 + }; + + return Matrix4D(tmp); +} + +Matrix4D Matrix4D::rotationY(float angle) { + // rotates this matrix around the y-axis + const float tmp[M_LENGTH] = { + Cos(angle), 0, Sin(angle), 0, + 0, 1, 0, 0, + -Sin(angle), 0, Cos(angle), 0, + 0, 0, 0, 1 + }; + + return Matrix4D(tmp); +} + +Matrix4D Matrix4D::rotationZ(float angle) { + // rotates this matrix around the z axis + const float tmp[M_LENGTH] = { + Cos(angle), -Sin(angle), 0, 0, + Sin(angle), Cos(angle), 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1 + }; + + return Matrix4D(tmp); +} + +Matrix4D Matrix4D::rotation(Vector3f axis, float angle) { + // assuming that axis is a unit vector + float x = axis.getX(); + float y = axis.getY(); + float z = axis.getZ(); + + const float tmp[M_LENGTH] = { + (x * x * (1 - Cos(angle)) + Cos(angle)), (x * y * (1 - Cos(angle)) - z * Sin(angle)), (x * z * (1 - Cos(angle)) + y * Sin(angle)), 0, + (x * y * (1 - Cos(angle)) + z * Sin(angle)), (y * y * (1 - Cos(angle)) + Cos(angle)), (y * z * (1 - Cos(angle)) - x * Sin(angle)), 0, + (x * z * (1 - Cos(angle)) - y * Sin(angle)), (y * z * (1 - Cos(angle)) + x * Sin(angle)), (z * z * (1 - Cos(angle)) + Cos(angle)), 0, + 0, 0, 0, 1 + }; + + return Matrix4D(tmp); +} + +Matrix4D Matrix4D::translation(float x, float y, float z) { + // gets a translation matrix from xyz coordinates + const float tmp[M_LENGTH] = { + 1, 0, 0, x, + 0, 1, 0, y, + 0, 0, 1, z, + 0, 0, 0, 1 + }; + + return Matrix4D(tmp); +} + diff --git a/cpp/localization/Matrix4D.h b/cpp/localization/Matrix4D.h new file mode 100644 index 0000000..927b6e4 --- /dev/null +++ b/cpp/localization/Matrix4D.h @@ -0,0 +1,224 @@ +#ifndef MATRIX4D_H_ +#define MATRIX4D_H_ + +#include "Vector3f.h" + +#define M_ROWS 4 +#define M_COLS 4 +#define M_LENGTH (M_ROWS * M_COLS) + +/** + * @class Matrix4D + * + * This class represents a 4x4 matrix and contains methods + * to operate on it + * + * @author Nuno Almeida (nuno.alm@ua.pt) + * Adapted - Miguel Abreu + */ +class Matrix4D { + +public: + float content[M_LENGTH]; // content of the matrix, vector-like + + /** + * Default constructor returns the identity matrix + */ + Matrix4D(); + + /** + * Constructor returns a matrix from a vector of floats + */ + Matrix4D(const float[]); + + /** + * Copy constructor + */ + Matrix4D(const Matrix4D& other); + + /** + * Constructor returns a translation matrix + */ + Matrix4D(const Vector3f& v); + + /** + * Destructor + */ + ~Matrix4D(); + + /** + * Sets a value in some position (vector-like) + */ + void set(unsigned i, float value); + + /** + * Sets a value in some position (matrix-like) + */ + void set(unsigned i, unsigned j, float value); + + /** + * Gets a value from some position (vector-like) + */ + float get(unsigned i) const; + + /** + * Gets a value from some position (matrix-like) + */ + float get(unsigned i, unsigned j) const; + + /** + * Assigns another matrix to this one + * + * @param other Another matrix + */ + void operator=(const Matrix4D& other); + + /** + * Gets the sum of another vector with this one + * + * @param other Another matrix + */ + Matrix4D operator+(const Matrix4D& other) const; + + /** + * Sums this matrix to another + * + * @param other Another matrix + * @return Sum of this matrix with another + */ + void operator+=(const Matrix4D& other); + + /** + * Gets the subtraction of another vector from this one + * + * @param other Another matrix + */ + Matrix4D operator-(const Matrix4D&) const; + + /** + * Subtracts another matrix from this one + * + * @param other Another matrix + * @return This matrix minus another + */ + void operator-=(const Matrix4D& other); + + /** + * Multiplies two matrices + * + * @param other Another matrix + * @return Multiplication matrix + */ + Matrix4D operator*(const Matrix4D& other) const; + + /** + * Multiplies a matrix with a vector + * + * @param other Another matrix + * @return Multiplication vector + */ + Vector3f operator*(const Vector3f& other) const; + + /** + * Checks whether this matrix is equal to another + * + * @param other Another matrix + * @return true/false + */ + bool operator==(const Matrix4D&) const; + + /** + * Gets the content of the position i (in vector representation) of this + * matrix + * + * @param pos Position + * @return Value in the position + */ + float& operator[](const unsigned pos); + + /** + * Gets the translation vector from this matrix + * + * @return Translation vector + */ + Vector3f toVector3f() const; + + /** + * Gets the transpose of this matrix + * + * @return Transpose + */ + Matrix4D transpose(); + + /** + * Gets the inverse of this matrix (m.abreu@2020) + * + * @param inverse_out inverse matrix + * @return true if it exists + */ + bool inverse(Matrix4D& inverse_out) const; + + /** + * Gets the inverse of this matrix, (m.abreu@2020) + * assuming that it represents an affine transformation with only translation and rotation + * This method creates a new matrix + * + * @return inverse matrix + */ + Matrix4D inverse_tranformation_matrix() const; + + /** + * Gets the inverse of this matrix, (m.abreu@2020) + * assuming that it represents an affine transformation with only translation and rotation + * This method overwrites the given matrix + * + * @param inverse_out inverse matrix output + */ + void inverse_tranformation_matrix(Matrix4D& inverse_out) const; + + + /** + * Gets the rotation matrix around x-axis + * + * @param angle Angle (degrees) + */ + static Matrix4D rotationX(float angle); + + /** + * Gets the rotation matrix around y-axis + * + * @param angle Angle (degrees) + */ + static Matrix4D rotationY(float angle); + + /** + * Gets the rotation matrix around z-axis + * + * @param angle Angle (degrees) + */ + static Matrix4D rotationZ(float angle); + + /** + * Gets the rotation matrix around an arbitrary axis + * + * @param axis Axis (x, y and z) + * @param angle Angle (degrees) + */ + static Matrix4D rotation(Vector3f axis, float angle); + + /** + * Gets the translation matrix + * + * @param x x-axis coordinate + * @param y y-axis coordinate + * @param z z-axis coordinate + */ + static Matrix4D translation(Vector3f v) { + return translation(v.getX(), v.getY(), v.getZ()); + } + static Matrix4D translation(float x, float y, float z); + + +}; + +#endif // MATRIX4D_H_ diff --git a/cpp/localization/RobovizLogger.cpp b/cpp/localization/RobovizLogger.cpp new file mode 100644 index 0000000..bf274d1 --- /dev/null +++ b/cpp/localization/RobovizLogger.cpp @@ -0,0 +1,131 @@ +#include +#include "RobovizLogger.h" +#include "robovizdraw.h" + +#define ROBOVIZ_HOST "localhost" +#define ROBOVIZ_PORT "32769" + + +RobovizLogger* RobovizLogger::Instance() { + static RobovizLogger instance; + return &instance; +} + +RobovizLogger::RobovizLogger() {} + +RobovizLogger::~RobovizLogger() { + destroy(); +} + +int RobovizLogger::init() { + if (is_initialized) return 0; + struct addrinfo hints; + int rv; + int numbytes; + + memset(&hints, 0, sizeof hints); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_DGRAM; + + if ((rv = getaddrinfo(ROBOVIZ_HOST, ROBOVIZ_PORT, &hints, &servinfo)) != 0) { + //fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(rv)); + return 1; + } + + // loop through all the results and make a socket + for (p = servinfo; p != NULL; p = p->ai_next) { + if ((sockfd = socket(p->ai_family, p->ai_socktype, + p->ai_protocol)) == -1) { + perror("socket"); + continue; + } + break; + } + + if (p == NULL) { + return 2; + } + + is_initialized = true; + return 0; +} + +void RobovizLogger::destroy() { + freeaddrinfo(servinfo); + servinfo=NULL; + close(sockfd); +} + +void RobovizLogger::swapBuffers(const string* setName) { + int bufSize = -1; + unsigned char* buf = newBufferSwap(setName, &bufSize); + sendto(sockfd, buf, bufSize, 0, p->ai_addr, p->ai_addrlen); + delete[] buf; +} + +void RobovizLogger::drawLine(float x1, float y1, float z1, float x2, float y2, float z2, + float thickness, float r, float g, float b, const string* setName) { + float pa[3] = {x1, y1, z1}; + float pb[3] = {x2, y2, z2}; + float color[3] = {r, g, b}; + + int bufSize = -1; + unsigned char* buf = newLine(pa, pb, thickness, color, setName, &bufSize); + sendto(sockfd, buf, bufSize, 0, p->ai_addr, p->ai_addrlen); + delete[] buf; +} + +void RobovizLogger::drawCircle(float x, float y, float radius, float thickness, + float r, float g, float b, const string* setName) { + float center[2] = {x, y}; + float color[3] = {r, g, b}; + + int bufSize = -1; + unsigned char* buf = newCircle(center, radius, thickness, color, setName, &bufSize); + sendto(sockfd, buf, bufSize, 0, p->ai_addr, p->ai_addrlen); + delete[] buf; +} + +void RobovizLogger::drawSphere(float x, float y, float z, float radius, + float r, float g, float b, const string* setName) { + float center[3] = {x, y, z}; + float color[3] = {r, g, b}; + + int bufSize = -1; + unsigned char* buf = newSphere(center, radius, color, setName, &bufSize); + sendto(sockfd, buf, bufSize, 0, p->ai_addr, p->ai_addrlen); + delete[] buf; +} + +void RobovizLogger::drawPoint(float x, float y, float z, float size, + float r, float g, float b, const string* setName) { + float center[3] = {x, y, z}; + float color[3] = {r, g, b}; + + int bufSize = -1; + unsigned char* buf = newPoint(center, size, color, setName, &bufSize); + sendto(sockfd, buf, bufSize, 0, p->ai_addr, p->ai_addrlen); + delete[] buf; +} + +void RobovizLogger::drawPolygon(const float* v, int numVerts, float r, float g, float b, + float a, const string* setName) { + float color[4] = {r, g, b, a}; + + int bufSize = -1; + unsigned char* buf = newPolygon(v, numVerts, color, setName, &bufSize); + sendto(sockfd, buf, bufSize, 0, p->ai_addr, p->ai_addrlen); + delete[] buf; +} + +void RobovizLogger::drawAnnotation(const string* text, float x, float y, float z, float r, + float g, float b, const string* setName) { + float color[3] = {r, g, b}; + float pos[3] = {x, y, z}; + + int bufSize = -1; + unsigned char* buf = newAnnotation(text, pos, color, setName, &bufSize); + sendto(sockfd, buf, bufSize, 0, p->ai_addr, p->ai_addrlen); + delete[] buf; +} + diff --git a/cpp/localization/RobovizLogger.h b/cpp/localization/RobovizLogger.h new file mode 100644 index 0000000..a858725 --- /dev/null +++ b/cpp/localization/RobovizLogger.h @@ -0,0 +1,58 @@ +/* + * RobovizLogger.h + * + * Created on: 2013/06/24 + * Author: Rui Ferreira + */ + +#ifndef _ROBOVIZLOGGER_H_ +#define _ROBOVIZLOGGER_H_ + +#define RobovizLoggerInstance RobovizLogger::Instance() + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class RobovizLogger { +private: + RobovizLogger(); + RobovizLogger(const RobovizLogger&); + RobovizLogger& operator=(const RobovizLogger&); + virtual ~RobovizLogger(); + bool is_initialized = false; + +public: + static RobovizLogger* Instance(); + + int init(); + void destroy(); + void swapBuffers(const std::string* setName); + + void drawLine(float x1, float y1, float z1, float x2, float y2, float z2, + float thickness, float r, float g, float b, const std::string* setName); + void drawCircle(float x, float y, float radius, float thickness, + float r, float g, float b, const std::string* setName); + void drawSphere(float x, float y, float z, float radius, + float r, float g, float b, const std::string* setName); + void drawPoint(float x, float y, float z, float size, + float r, float g, float b, const std::string* setName); + void drawPolygon(const float* v, int numVerts, float r, float g, float b, + float a, const std::string* setName); + void drawAnnotation(const std::string* text, float x, float y, float z, float r, + float g, float b, const std::string* setName); + +private: + int sockfd; + struct addrinfo* p; + struct addrinfo* servinfo; +}; + +#endif // _ROBOVIZLOGGER_H_ diff --git a/cpp/localization/Singleton.h b/cpp/localization/Singleton.h new file mode 100644 index 0000000..12bdf8e --- /dev/null +++ b/cpp/localization/Singleton.h @@ -0,0 +1,20 @@ +#ifndef SINGLETON_H +#define SINGLETON_H + +template +class Singleton { +public: + + static T& getInstance() { + static T instance; + return instance; + } + +private: + Singleton(); + ~Singleton(); + Singleton(Singleton const&); + Singleton& operator=(Singleton const&); +}; + +#endif // SINGLETON_H diff --git a/cpp/localization/Vector3f.cpp b/cpp/localization/Vector3f.cpp new file mode 100644 index 0000000..c317a7f --- /dev/null +++ b/cpp/localization/Vector3f.cpp @@ -0,0 +1,197 @@ +#include "Vector3f.h" + + +using namespace std; + +Vector3f::Vector3f() { + x = 0.0; + y = 0.0; + z = 0.0; +} + +Vector3f::Vector3f(float x, float y, float z) { + this->x = x; + this->y = y; + this->z = z; +} + +Vector3f::Vector3f(const Vector3f& other) { + x = other.x; + y = other.y; + z = other.z; +} + +Vector3f::Vector3f(const Vector& other) { + x = other.x; + y = other.y; + z = 0.0; +} + +Vector3f::~Vector3f() { +} + +float Vector3f::getX() const { + return x; +} + +void Vector3f::setX(float x) { + this->x = x; +} + +float Vector3f::getY() const { + return y; +} + +void Vector3f::setY(float y) { + this->y = y; +} + +float Vector3f::getZ() const { + return z; +} + +void Vector3f::setZ(float z) { + this->z = z; +} + +float Vector3f::operator[](const int index) const { + float val=0.0; + switch (index) { + case 0: val = x; + break; + case 1: val = y; + break; + case 2: val = z; + break; + } + return val; +} + +Vector3f Vector3f::operator+(const Vector3f& other) const { + return Vector3f(x + other.x, y + other.y, z + other.z); +} + +Vector3f Vector3f::operator-(const Vector3f& other) const { + return Vector3f(x - other.x, y - other.y, z - other.z); +} + +Vector3f Vector3f::operator-() const { + return Vector3f() - * this; +} + +Vector3f Vector3f::operator*(const Vector3f& other) const { + return Vector3f(x * other.x, y * other.y, z * other.z); +} + +Vector3f Vector3f::operator/(const Vector3f& other) const { + return Vector3f(x / other.x, y / other.y, z / other.z); +} + +bool Vector3f::operator==(const Vector3f& other) const { + return x == other.x && y == other.y && z == other.z; +} + +Vector3f Vector3f::operator/(float factor) const { + return Vector3f(x / factor, y / factor, z / factor); +} + +Vector3f Vector3f::operator+(float factor) const { + return Vector3f(x + factor, y + factor, z + factor); +} + +Vector3f Vector3f::operator%(float factor) const { + return Vector3f(fmod(x, factor), fmod(y, factor), fmod(z, factor)); +} + +Vector3f Vector3f::operator*(float factor) const { + return Vector3f(x * factor, y * factor, z * factor); +} + +Vector3f Vector3f::operator+=(const Vector3f& other) { + x += other.x; + y += other.y; + z += other.z; + + return *this; +} + +Vector3f Vector3f::operator+=(float factor) { + x += factor; + y += factor; + z += factor; + + return *this; +} + +Vector3f Vector3f::operator-=(const Vector3f& other) { + x -= other.x; + y -= other.y; + z -= other.z; + + return *this; +} + +Vector3f Vector3f::operator-=(float factor) { + x -= factor; + y -= factor; + z -= factor; + + return *this; +} + +Vector3f Vector3f::operator/=(float factor) { + x /= factor; + y /= factor; + z /= factor; + + return *this; +} + + +float Vector3f::innerProduct(const Vector3f& other) const { + return x * other.x + y * other.y + z * other.z; +} + +Vector3f Vector3f::crossProduct(const Vector3f& other) const { + Vector3f aux; + + aux.x = this->y * other.z - this->z * other.y; + aux.y = this->z * other.x - this->x * other.z; + aux.z = this->x * other.y - this->y * other.x; + + return aux; +} + +float Vector3f::length() const { + return sqrt(x * x + y * y + z * z); +} + + +Vector3f Vector3f::normalize(float len) const { + return (*this) * (len / this->length()); +} + +Vector3f Vector3f::toCartesian() const { + // x = distance + // y = theta + // z = phi + return Vector3f(x * Cos(z) * Cos(y), x * Cos(z) * Sin(y), x * Sin(z)); +} + +Vector3f Vector3f::toPolar() const { + return Vector3f(this->length(), // distance + ATan2(y, x), // theta + ATan2(z, sqrt(x * x + y * y))); // phi +} + +float Vector3f::dist(const Vector3f &other) const { + return (*this -other).length(); +} + +Vector Vector3f::to2d() const { + return Vector(x, y); +} + +Vector3f Vector3f::determineMidpoint(Vector3f a, Vector3f b) { + return (a+b)/2; /* m.abreu@2020 */ +} diff --git a/cpp/localization/Vector3f.h b/cpp/localization/Vector3f.h new file mode 100644 index 0000000..4dfd7c8 --- /dev/null +++ b/cpp/localization/Vector3f.h @@ -0,0 +1,187 @@ +#ifndef VECTOR3F_H +#define VECTOR3F_H + +#include +#include "Geometry.h" +#include +//! Describes a vector of three floats + +/*! + * \author Hugo Picado (hugopicado@ua.pt) + * \author Nuno Almeida (nuno.alm@ua.pt) + * Adapted - Miguel Abreu + */ +class Vector3f { +public: + //! Default constructor + Vector3f(); + + /*! + * Constructor + * + * \param x x-axis coordinate + * \param y y-axis coordinate + * \param z z-axis coordinate + */ + Vector3f(float x, float y, float z); + + //! Copy constructor + Vector3f(const Vector3f& other); + + Vector3f(const Vector& other); + + //! Destructor + ~Vector3f(); + + //! getX + float getX() const; + void setX(float x); + + //! getY + float getY() const; + void setY(float y); + + //! getZ + float getZ() const; + void setZ(float z); + + //! Access X Y Z through indexes 0 1 2 + float operator[](const int) const; + + //! Sums this vector to another + Vector3f operator+(const Vector3f& other) const; + + //! Subtracts another vector from this + Vector3f operator-(const Vector3f& other) const; + + //! Negates the vector + Vector3f operator-() const; + + //! Multiples this vector by another + Vector3f operator*(const Vector3f& other) const; + + //! Divides this vector by another + Vector3f operator/(const Vector3f& other) const; + + bool operator==(const Vector3f& other) const; + + /*! + * Multiples this vector by a scalar + * + * \param factor Scalar number + */ + Vector3f operator*(float factor) const; + + /*! + * Divides this vector by a scalar + * + * \param factor Scalar number + */ + Vector3f operator/(float factor) const; + + /*! + * Add this vector to a scalar + * + * \param factor Scalar number + */ + Vector3f operator+(float factor) const; + + /*! + * Integer remainder this vector by a scalar + * + * \param factor Scalar number + */ + Vector3f operator%(float factor) const; + + /** + * Sums this vector to another assuming the value of the result + * + * \param other Vector3f + */ + Vector3f operator+=(const Vector3f& other); + + /** + * Add this vector to a scalar assuming the value of the result + * + * \param factor Scalar number + */ + Vector3f operator+=(float factor); + + /** + * Subtracts other vector from this vector assuming the value of the result + * + * \param other Vector3f + */ + Vector3f operator-=(const Vector3f& other); + + /** + * Subtracts a scalar from this vector assuming the value of the result + * + * \param factor Scalar number + */ + Vector3f operator-=(float factor); + + /*! + * Divides this vector by a scalar assuming the value of the result + * + * \param factor Scalar number + */ + Vector3f operator/=(float factor); + + /*! + * Computes the inner product of this vector with another + * + * \param other Vector to compute the inner product + * \return Resultant vector + */ + float innerProduct(const Vector3f& other) const; + + /*! + * Computes the cross product of this vector with another + * + * \param other Vector to compute the cross product + * \return Resultant vector + */ + Vector3f crossProduct(const Vector3f& other) const; + + /*! + * Gets the length of the vector + * + * \return Length of the vector + */ + float length() const; + + /*! + * Normalizes the vector to an arbitrary length (default is 1) + * + * \param len Length + */ + Vector3f normalize(float len = 1) const; + /*! + * Converts the vector coordinates from polar to cartesian + * It is assumed that the vector has the angular coordinates in degrees + */ + Vector3f toCartesian() const; + + //! Converts the vector coordinates from cartesian to polar + Vector3f toPolar() const; + + //! Converts the 3d vector to a 2d vector + Vector to2d() const; + + //! Gets the distance between this vector and another + float dist(const Vector3f& other) const; + + /* !Determines the midpoint between 2 points in a 3D space + * + **/ + static Vector3f determineMidpoint(Vector3f a, Vector3f b); + +public: + float x; // x-axis coordinate + float y; // y-axis coordinate + float z; // z-axis coordinate + +}; + +#endif // VECTOR3F_H diff --git a/cpp/localization/World.h b/cpp/localization/World.h new file mode 100644 index 0000000..5a62b5f --- /dev/null +++ b/cpp/localization/World.h @@ -0,0 +1,56 @@ +/** + * FILENAME: World + * DESCRIPTION: World data from Python + * AUTHOR: Miguel Abreu (m.abreu@fe.up.pt) + * DATE: 2021 + */ + +#pragma once +#include "Vector3f.h" +#include "Singleton.h" +#include "Matrix4D.h" +#include "Line6f.h" +#include +#include + +using namespace std; + + +class World { + friend class Singleton; + +private: + + World(){}; + +public: + + //Feet variables: (0) left, (1) right + bool foot_touch[2]; // is foot touching ground + Vector3f foot_contact_rel_pos[2]; // foot_transform * translation(foot_contact_pt) + + bool ball_seen; + Vector3f ball_rel_pos_cart; + Vector3f ball_cheat_abs_cart_pos; + Vector3f my_cheat_abs_cart_pos; + + struct sLMark { + bool seen; + bool isCorner; + Vector3f pos; + Vector3f rel_pos; + }; + + sLMark landmark[8]; + + struct sLine { + Vector3f start, end; + sLine(const Vector3f& s, const Vector3f& e) : start(s), end(e) {}; + }; + + vector lines_polar; + + +}; + +typedef Singleton SWorld; \ No newline at end of file diff --git a/cpp/localization/debug_main.cc b/cpp/localization/debug_main.cc new file mode 100644 index 0000000..423a7f2 --- /dev/null +++ b/cpp/localization/debug_main.cc @@ -0,0 +1,196 @@ +#include +#include "Geometry.h" +#include "Vector3f.h" +#include "Matrix4D.h" +#include "FieldNoise.h" +#include "Line6f.h" +#include "World.h" +#include "Field.h" +#include "LocalizerV2.h" + +using namespace std; + +static LocalizerV2& loc = SLocalizerV2::getInstance(); + +void print_python_data(){ + + static World &world = SWorld::getInstance(); + + cout << "Foot touch: " << world.foot_touch[0] << " " << world.foot_touch[1] << endl; + cout << "LFoot contact rpos: " << world.foot_contact_rel_pos[0].x << " " << world.foot_contact_rel_pos[0].y << " " << world.foot_contact_rel_pos[0].z << endl; + cout << "RFoot contact rpos: " << world.foot_contact_rel_pos[1].x << " " << world.foot_contact_rel_pos[1].y << " " << world.foot_contact_rel_pos[1].z << endl; + cout << "Ball seen: " << world.ball_seen << endl; + cout << "Ball rpos cart: " << world.ball_rel_pos_cart.x << " " << world.ball_rel_pos_cart.y << " " << world.ball_rel_pos_cart.z << endl; + cout << "Ball cheat: " << world.ball_cheat_abs_cart_pos.x << " " << world.ball_cheat_abs_cart_pos.y << " " << world.ball_cheat_abs_cart_pos.z << endl; + cout << "Me cheat: " << world.my_cheat_abs_cart_pos.x << " " << world.my_cheat_abs_cart_pos.y << " " << world.my_cheat_abs_cart_pos.z << endl; + + for(int i=0; i<8; i++){ + cout << "Landmark " << i << ": " << + world.landmark[i].seen << " " << + world.landmark[i].isCorner << " " << + world.landmark[i].pos.x << " " << + world.landmark[i].pos.y << " " << + world.landmark[i].pos.z << " " << + world.landmark[i].rel_pos.x << " " << + world.landmark[i].rel_pos.y << " " << + world.landmark[i].rel_pos.z << endl; + } + + for(int i=0; i +#include "Geometry.h" +#include "Vector3f.h" +#include "Matrix4D.h" +#include "FieldNoise.h" +#include "Line6f.h" +#include "World.h" +#include "Field.h" +#include "LocalizerV2.h" +#include +#include + +namespace py = pybind11; +using namespace std; + +static LocalizerV2& loc = SLocalizerV2::getInstance(); + +void print_python_data(){ + + static World &world = SWorld::getInstance(); + + cout << "Foot touch: " << world.foot_touch[0] << " " << world.foot_touch[1] << endl; + cout << "LFoot contact rpos: " << world.foot_contact_rel_pos[0].x << " " << world.foot_contact_rel_pos[0].y << " " << world.foot_contact_rel_pos[0].z << endl; + cout << "RFoot contact rpos: " << world.foot_contact_rel_pos[1].x << " " << world.foot_contact_rel_pos[1].y << " " << world.foot_contact_rel_pos[1].z << endl; + cout << "Ball seen: " << world.ball_seen << endl; + cout << "Ball rpos cart: " << world.ball_rel_pos_cart.x << " " << world.ball_rel_pos_cart.y << " " << world.ball_rel_pos_cart.z << endl; + cout << "Ball cheat: " << world.ball_cheat_abs_cart_pos.x << " " << world.ball_cheat_abs_cart_pos.y << " " << world.ball_cheat_abs_cart_pos.z << endl; + cout << "Me cheat: " << world.my_cheat_abs_cart_pos.x << " " << world.my_cheat_abs_cart_pos.y << " " << world.my_cheat_abs_cart_pos.z << endl; + + for(int i=0; i<8; i++){ + cout << "Landmark " << i << ": " << + world.landmark[i].seen << " " << + world.landmark[i].isCorner << " " << + world.landmark[i].pos.x << " " << + world.landmark[i].pos.y << " " << + world.landmark[i].pos.z << " " << + world.landmark[i].rel_pos.x << " " << + world.landmark[i].rel_pos.y << " " << + world.landmark[i].rel_pos.z << endl; + } + + for(int i=0; i compute( + bool lfoot_touch, bool rfoot_touch, + py::array_t feet_contact, + bool ball_seen, py::array_t ball_pos, + py::array_t me_pos, + py::array_t landmarks, + py::array_t lines){ + + // ================================================= 1. Parse data + + static World &world = SWorld::getInstance(); + world.foot_touch[0] = lfoot_touch; + world.foot_touch[1] = rfoot_touch; + + //Structure of feet_contact {lfoot_contact_pt, rfoot_contact_pt, lfoot_contact_rel_pos, rfoot_contact_rel_pos} + + py::buffer_info feet_contact_buf = feet_contact.request(); + double *feet_contact_ptr = (double *) feet_contact_buf.ptr; + world.foot_contact_rel_pos[0].x = feet_contact_ptr[0]; + world.foot_contact_rel_pos[0].y = feet_contact_ptr[1]; + world.foot_contact_rel_pos[0].z = feet_contact_ptr[2]; + world.foot_contact_rel_pos[1].x = feet_contact_ptr[3]; + world.foot_contact_rel_pos[1].y = feet_contact_ptr[4]; + world.foot_contact_rel_pos[1].z = feet_contact_ptr[5]; + + world.ball_seen = ball_seen; + + //Structure of ball_pos {ball_rel_pos_cart, ball_cheat_abs_cart_pos} + + py::buffer_info ball_pos_buf = ball_pos.request(); + double *ball_pos_ptr = (double *) ball_pos_buf.ptr; + world.ball_rel_pos_cart.x = ball_pos_ptr[0]; + world.ball_rel_pos_cart.y = ball_pos_ptr[1]; + world.ball_rel_pos_cart.z = ball_pos_ptr[2]; + world.ball_cheat_abs_cart_pos.x = ball_pos_ptr[3]; + world.ball_cheat_abs_cart_pos.y = ball_pos_ptr[4]; + world.ball_cheat_abs_cart_pos.z = ball_pos_ptr[5]; + + py::buffer_info me_pos_buf = me_pos.request(); + double *me_pos_ptr = (double *) me_pos_buf.ptr; + world.my_cheat_abs_cart_pos.x = me_pos_ptr[0]; + world.my_cheat_abs_cart_pos.y = me_pos_ptr[1]; + world.my_cheat_abs_cart_pos.z = me_pos_ptr[2]; + + py::buffer_info landmarks_buf = landmarks.request(); + double *landmarks_ptr = (double *) landmarks_buf.ptr; + + for(int i=0; i<8; i++){ + world.landmark[i].seen = (bool) landmarks_ptr[0]; + world.landmark[i].isCorner = (bool) landmarks_ptr[1]; + world.landmark[i].pos.x = landmarks_ptr[2]; + world.landmark[i].pos.y = landmarks_ptr[3]; + world.landmark[i].pos.z = landmarks_ptr[4]; + world.landmark[i].rel_pos.x = landmarks_ptr[5]; + world.landmark[i].rel_pos.y = landmarks_ptr[6]; + world.landmark[i].rel_pos.z = landmarks_ptr[7]; + landmarks_ptr += 8; + } + + py::buffer_info lines_buf = lines.request(); + int lines_len = lines_buf.shape[0]; + double *lines_ptr = (double *) lines_buf.ptr; + world.lines_polar.clear(); + + for(int i=0; i retval = py::array_t(35); //allocate + py::buffer_info buff = retval.request(); + float *ptr = (float *) buff.ptr; + + for(int i=0; i<16; i++){ + ptr[i] = loc.headTofieldTransform.content[i]; + } + ptr += 16; + for(int i=0; i<16; i++){ + ptr[i] = loc.fieldToheadTransform.content[i]; + } + ptr += 16; + + ptr[0] = (float) loc.is_uptodate; + ptr[1] = loc.head_z; + ptr[2] = (float) loc.is_head_z_uptodate; + + + return retval; +} + +void print_report(){ + loc.print_report(); +} + +void draw_visible_elements(bool is_right_side){ + Field& fd = SField::getInstance(); + fd.draw_visible(loc.headTofieldTransform, is_right_side); +} + + +using namespace pybind11::literals; //to add informative argument names as -> "argname"_a + +PYBIND11_MODULE(localization, m) { //the python module name, m is the interface to create bindings + m.doc() = "Probabilistic 6D localization algorithm"; // optional module docstring + + //optional arguments names + m.def("compute", &compute, "Compute the 6D pose based on visual information and return transformation matrices and other relevant data", + "lfoot_touch"_a, + "rfoot_touch"_a, + "feet_contact"_a, + "ball_seen"_a, + "ball_pos"_a, + "me_pos"_a, + "landmarks"_a, + "lines"_a); + + m.def("print_python_data", &print_python_data, "Print data received from Python"); + m.def("print_report", &print_report, "Print localization report"); + m.def("draw_visible_elements", &draw_visible_elements, "Draw all visible elements in RoboViz", "is_right_side"_a); + +} + diff --git a/cpp/localization/robovizdraw.h b/cpp/localization/robovizdraw.h new file mode 100644 index 0000000..fe0f49d --- /dev/null +++ b/cpp/localization/robovizdraw.h @@ -0,0 +1,208 @@ +/* + * Copyright (C) 2011 Justin Stoecker + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef _ROBOVIZDRAW_H_ +#define _ROBOVIZDRAW_H_ + +#include +#include +#include + +using namespace std; + +inline int writeCharToBuf(unsigned char* buf, unsigned char value) { + *buf = value; + return 1; +} + +inline int writeFloatToBuf(unsigned char* buf, float value) { + char temp[20]; + sprintf(temp, "%6f", value); + memcpy(buf, temp, 6); + return 6; +} + +inline int writeColorToBuf(unsigned char* buf, const float* color, int channels) { + int i; + for (i = 0; i < channels; i++) + writeCharToBuf(buf + i, (unsigned char) (color[i]*255)); + return i; +} + +inline int writeStringToBuf(unsigned char* buf, const string* text) { + long i = 0; + if (text != NULL) + i += text->copy((char*) buf + i, text->length(), 0); + i += writeCharToBuf(buf + i, 0); + return i; +} + +unsigned char* newBufferSwap(const string* name, int* bufSize) { + *bufSize = 3 + ((name != NULL) ? name->length() : 0); + unsigned char* buf = new unsigned char[*bufSize]; + + long i = 0; + i += writeCharToBuf(buf + i, 0); + i += writeCharToBuf(buf + i, 0); + i += writeStringToBuf(buf + i, name); + + return buf; +} + +unsigned char* newCircle(const float* center, float radius, float thickness, + const float* color, const string* setName, int* bufSize) { + + *bufSize = 30 + ((setName != NULL) ? setName->length() : 0); + unsigned char* buf = new unsigned char[*bufSize]; + + long i = 0; + i += writeCharToBuf(buf + i, 1); + i += writeCharToBuf(buf + i, 0); + i += writeFloatToBuf(buf + i, center[0]); + i += writeFloatToBuf(buf + i, center[1]); + i += writeFloatToBuf(buf + i, radius); + i += writeFloatToBuf(buf + i, thickness); + i += writeColorToBuf(buf + i, color, 3); + i += writeStringToBuf(buf + i, setName); + + return buf; +} + +unsigned char* newLine(const float* a, const float* b, float thickness, + const float* color, const string* setName, int* bufSize) { + + *bufSize = 48 + ((setName != NULL) ? setName->length() : 0); + unsigned char* buf = new unsigned char[*bufSize]; + + long i = 0; + i += writeCharToBuf(buf + i, 1); + i += writeCharToBuf(buf + i, 1); + i += writeFloatToBuf(buf + i, a[0]); + i += writeFloatToBuf(buf + i, a[1]); + i += writeFloatToBuf(buf + i, a[2]); + i += writeFloatToBuf(buf + i, b[0]); + i += writeFloatToBuf(buf + i, b[1]); + i += writeFloatToBuf(buf + i, b[2]); + i += writeFloatToBuf(buf + i, thickness); + i += writeColorToBuf(buf + i, color, 3); + i += writeStringToBuf(buf + i, setName); + + return buf; +} + +unsigned char* newPoint(const float* p, float size, const float* color, + const string* setName, int* bufSize) { + + *bufSize = 30 + ((setName != NULL) ? setName->length() : 0); + unsigned char* buf = new unsigned char[*bufSize]; + + long i = 0; + i += writeCharToBuf(buf + i, 1); + i += writeCharToBuf(buf + i, 2); + i += writeFloatToBuf(buf + i, p[0]); + i += writeFloatToBuf(buf + i, p[1]); + i += writeFloatToBuf(buf + i, p[2]); + i += writeFloatToBuf(buf + i, size); + i += writeColorToBuf(buf + i, color, 3); + i += writeStringToBuf(buf + i, setName); + + return buf; +} + +unsigned char* newSphere(const float* p, float radius, const float* color, + const string* setName, int* bufSize) { + + *bufSize = 30 + ((setName != NULL) ? setName->length() : 0); + unsigned char* buf = new unsigned char[*bufSize]; + + long i = 0; + i += writeCharToBuf(buf + i, 1); + i += writeCharToBuf(buf + i, 3); + i += writeFloatToBuf(buf + i, p[0]); + i += writeFloatToBuf(buf + i, p[1]); + i += writeFloatToBuf(buf + i, p[2]); + i += writeFloatToBuf(buf + i, radius); + i += writeColorToBuf(buf + i, color, 3); + i += writeStringToBuf(buf + i, setName); + + return buf; +} + +unsigned char* newPolygon(const float* v, int numVerts, const float* color, + const string* setName, int* bufSize) { + + *bufSize = 18 * numVerts + 8 + ((setName != NULL) ? setName->length() : 0); + unsigned char* buf = new unsigned char[*bufSize]; + + long i = 0; + i += writeCharToBuf(buf + i, 1); + i += writeCharToBuf(buf + i, 4); + i += writeCharToBuf(buf + i, numVerts); + i += writeColorToBuf(buf + i, color, 4); + + for (int j = 0; j < numVerts; j++) { + i += writeFloatToBuf(buf + i, v[j * 3 + 0]); + i += writeFloatToBuf(buf + i, v[j * 3 + 1]); + i += writeFloatToBuf(buf + i, v[j * 3 + 2]); + } + + i += writeStringToBuf(buf + i, setName); + + return buf; +} + +unsigned char* newAnnotation(const string* text, const float* p, + const float* color, const string* setName, int* bufSize) { + + *bufSize = 25 + text->length() + ((setName != NULL) ? setName->length() : 0); + unsigned char* buf = new unsigned char[*bufSize]; + + long i = 0; + i += writeCharToBuf(buf + i, 2); + i += writeCharToBuf(buf + i, 0); + i += writeFloatToBuf(buf + i, p[0]); + i += writeFloatToBuf(buf + i, p[1]); + i += writeFloatToBuf(buf + i, p[2]); + i += writeColorToBuf(buf + i, color, 3); + i += writeStringToBuf(buf + i, text); + i += writeStringToBuf(buf + i, setName); + + return buf; +} + +unsigned char* newAgentAnnotation(const string* text, bool leftTeam, + int agentNum, const float* color, int* bufSize) { + + *bufSize = (text == NULL) ? 3 : 7 + text->length(); + unsigned char* buf = new unsigned char[*bufSize]; + + long i = 0; + i += writeCharToBuf(buf + i, 2); + + if (text == NULL) { + i += writeCharToBuf(buf + i, 2); + i += writeCharToBuf(buf + i, (leftTeam ? agentNum - 1 : agentNum + 127)); + } else { + i += writeCharToBuf(buf + i, 1); + i += writeCharToBuf(buf + i, (leftTeam ? agentNum - 1 : agentNum + 127)); + i += writeColorToBuf(buf + i, color, 3); + i += writeStringToBuf(buf + i, text); + } + + return buf; +} + +#endif diff --git a/example.py b/example.py new file mode 100644 index 0000000..15ece14 --- /dev/null +++ b/example.py @@ -0,0 +1,30 @@ +from agent.Base_Agent import Base_Agent as Agent +from math_ops.Math_Ops import Math_Ops as M +from scripts.commons.Script import Script + +script = Script() +a = script.args + +# Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name +player = Agent(a.i, a.p, a.m, a.u, a.r, a.t, enable_draw=True) +w = player.world +player.scom.unofficial_beam((-3,0,w.robot.beam_height), 0) +getting_up = False +while True: + player_2d = w.robot.loc_head_position[:2] + ball_2d = w.ball_abs_pos[:2] + goal_dir = M.vector_angle( (15,0)-player_2d ) # Goal direction + if player.behavior.is_ready("Get_Up") or getting_up: + getting_up = not player.behavior.execute("Get_Up") # True on completion + else: + if ball_2d[0] > 0: # kick if ball is on opponent's side (x>0) + player.behavior.execute("Basic_Kick", goal_dir) + elif M.distance_point_to_segment(player_2d,ball_2d, ball_2d+ M.normalize_vec( ball_2d-(15,0) ) ) > 0.1: # not aligned + next_pos, next_ori, dist = player.path_manager.get_path_to_ball(x_ori=goal_dir, x_dev=-0.3, torso_ori=goal_dir) + player.behavior.execute("Walk", next_pos, True, next_ori, True, dist) + else: # Robot is aligned + player.behavior.execute("Walk", (15,0), True, goal_dir, True, 0.5) + player.scom.commit_and_send( w.robot.get_command() ) + player.scom.receive() +w.draw.annotation((*player_2d,0.6),"Hello!",w.draw.Color.white,"my_info",flush=False) +w.draw.line(player_2d, ball_2d, 3, w.draw.Color.yellow, "my_info", flush=True) \ No newline at end of file diff --git a/kill.sh b/kill.sh new file mode 100755 index 0000000..c941e1f --- /dev/null +++ b/kill.sh @@ -0,0 +1,2 @@ +#!/bin/bash +pkill -9 -e -f "python3 ./Run_" diff --git a/logs/Logger.py b/logs/Logger.py new file mode 100644 index 0000000..2eb3152 --- /dev/null +++ b/logs/Logger.py @@ -0,0 +1,48 @@ +from pathlib import Path +from datetime import datetime +import random +from string import ascii_uppercase + +class Logger(): + _folder = None + + def __init__(self, is_enabled:bool, topic:str) -> None: + self.no_of_entries = 0 + self.enabled = is_enabled + self.topic = topic + + def write(self, msg:str, timestamp:bool=True, step:int=None) -> None: + ''' + Write `msg` to file named `self.topic` + + Parameters + ---------- + msg : str + message to be written + step : int + simulation step is written before the message to provide additional information + default is `None` (nothing is written before the message) + ''' + if not self.enabled: return + + # The log folder is only created if needed + if Logger._folder is None: + rnd = ''.join(random.choices(ascii_uppercase, k=6)) # Useful if multiple processes are running in parallel + Logger._folder = "./logs/" + datetime.now().strftime("%Y-%m-%d_%H.%M.%S__") + rnd + "/" + print("\nLogger Info: see",Logger._folder) + Path(Logger._folder).mkdir(parents=True, exist_ok=True) + + self.no_of_entries += 1 + + with open(Logger._folder + self.topic + ".log", 'a+') as f: + prefix = "" + write_step = step is not None + if timestamp or write_step: + prefix = "{" + if timestamp: + prefix += datetime.now().strftime("%a %H:%M:%S") + if write_step: prefix += " " + if write_step: + prefix += f'Step:{step}' + prefix += "} " + f.write(prefix + msg + "\n") \ No newline at end of file diff --git a/math_ops/Inverse_Kinematics.py b/math_ops/Inverse_Kinematics.py new file mode 100644 index 0000000..02446e6 --- /dev/null +++ b/math_ops/Inverse_Kinematics.py @@ -0,0 +1,242 @@ +from math import asin, atan, atan2, pi, sqrt +from math_ops.Matrix_3x3 import Matrix_3x3 +from math_ops.Math_Ops import Math_Ops as M +import numpy as np + +class Inverse_Kinematics(): + + # leg y deviation, upper leg height, upper leg depth, lower leg length, knee extra angle, max ankle z + NAO_SPECS_PER_ROBOT = ((0.055, 0.12, 0.005, 0.1, atan(0.005/0.12), -0.091), + (0.055, 0.13832, 0.005, 0.11832, atan(0.005/0.13832), -0.106), + (0.055, 0.12, 0.005, 0.1, atan(0.005/0.12), -0.091), + (0.072954143,0.147868424, 0.005, 0.127868424, atan(0.005/0.147868424), -0.114), + (0.055, 0.12, 0.005, 0.1, atan(0.005/0.12), -0.091)) + + TORSO_HIP_Z = 0.115 # distance in the z-axis, between the torso and each hip (same for all robots) + TORSO_HIP_X = 0.01 # distance in the x-axis, between the torso and each hip (same for all robots) (hip is 0.01m to the back) + + def __init__(self, robot) -> None: + self.robot = robot + self.NAO_SPECS = Inverse_Kinematics.NAO_SPECS_PER_ROBOT[robot.type] + + def torso_to_hip_transform(self, coords, is_batch=False): + ''' + Convert cartesian coordinates that are relative to torso to coordinates that are relative the center of both hip joints + + Parameters + ---------- + coords : array_like + One 3D position or list of 3D positions + is_batch : `bool` + Indicates if coords is a batch of 3D positions + + Returns + ------- + coord : `list` or ndarray + A numpy array is returned if is_batch is False, otherwise, a list of arrays is returned + ''' + if is_batch: + return [c + (Inverse_Kinematics.TORSO_HIP_X, 0, Inverse_Kinematics.TORSO_HIP_Z) for c in coords] + else: + return coords + (Inverse_Kinematics.TORSO_HIP_X, 0, Inverse_Kinematics.TORSO_HIP_Z) + + + def head_to_hip_transform(self, coords, is_batch=False): + ''' + Convert cartesian coordinates that are relative to head to coordinates that are relative the center of both hip joints + + Parameters + ---------- + coords : array_like + One 3D position or list of 3D positions + is_batch : `bool` + Indicates if coords is a batch of 3D positions + + Returns + ------- + coord : `list` or ndarray + A numpy array is returned if is_batch is False, otherwise, a list of arrays is returned + ''' + coords_rel_torso = self.robot.head_to_body_part_transform( "torso", coords, is_batch ) + return self.torso_to_hip_transform(coords_rel_torso, is_batch) + + def get_body_part_pos_relative_to_hip(self, body_part_name): + ''' Get body part position relative to the center of both hip joints ''' + bp_rel_head = self.robot.body_parts[body_part_name].transform.get_translation() + return self.head_to_hip_transform(bp_rel_head) + + def get_ankle_pos_relative_to_hip(self, is_left): + ''' Internally calls get_body_part_pos_relative_to_hip() ''' + return self.get_body_part_pos_relative_to_hip("lankle" if is_left else "rankle") + + def get_linear_leg_trajectory(self, is_left:bool, p1, p2=None, foot_ori3d=(0,0,0), dynamic_pose:bool=True, resolution=100): + ''' + Compute leg trajectory so that the ankle moves linearly between two 3D points (relative to hip) + + Parameters + ---------- + is_left : `bool` + set to True to select left leg, False to select right leg + p1 : array_like, length 3 + if p2 is None: + p1 is the target position (relative to hip), and the initial point is given by the ankle's current position + if p2 is not None: + p1 is the initial point (relative to hip) + p2 : array_like, length 3 / `None` + target position (relative to hip) or None (see p1) + foot_ori3d : array_like, length 3 + rotation around x,y,z (rotation around x & y are biases, relative to a vertical pose, or dynamic pose, if enabled) + dynamic_pose : `bool` + enable dynamic feet rotation to be parallel to the ground, based on IMU + resolution : int + interpolation resolution; more resolution is always better, but it takes more time to compute; + having more points does not make the movement slower, because if there are excessive points they are removed + during the analytical optimization + + Returns + ------- + trajecory : `tuple` + indices, [[values_1,error_codes_1], [values_2,error_codes_2], ...] + See leg() for further details + ''' + + if p2 is None: + p2 = np.asarray(p1, float) + p1 = self.get_body_part_pos_relative_to_hip('lankle' if is_left else 'rankle') + else: + p1 = np.asarray(p1, float) + p2 = np.asarray(p2, float) + + vec = (p2 - p1) / resolution + + + hip_points = [p1 + vec * i for i in range(1,resolution+1)] + interpolation = [self.leg(p, foot_ori3d, is_left, dynamic_pose) for p in hip_points] + + indices = [2,4,6,8,10,12] if is_left else [3,5,7,9,11,13] + + last_joint_values = self.robot.joints_position[indices[0:4]] #exclude feet joints to compute ankle trajectory + next_step = interpolation[0] + trajectory = [] + + for p in interpolation[1:-1]: + if np.any(np.abs(p[1][0:4]-last_joint_values) > 7.03): + trajectory.append(next_step[1:3]) + last_joint_values = next_step[1][0:4] + next_step = p + else: + next_step = p + + trajectory.append(interpolation[-1][1:3]) + + return indices, trajectory + + + + def leg(self, ankle_pos3d, foot_ori3d, is_left:bool, dynamic_pose:bool): + ''' + Compute inverse kinematics for the leg, considering as input the relative 3D position of the ankle and 3D orientation* of the foot + *the yaw can be controlled directly, but the pitch and roll are biases (see below) + + Parameters + ---------- + ankle_pos3d : array_like, length 3 + (x,y,z) position of ankle in 3D, relative to the center of both hip joints + foot_ori3d : array_like, length 3 + rotation around x,y,z (rotation around x & y are biases, relative to a vertical pose, or dynamic pose, if enabled) + is_left : `bool` + set to True to select left leg, False to select right leg + dynamic_pose : `bool` + enable dynamic feet rotation to be parallel to the ground, based on IMU + + Returns + ------- + indices : `list` + indices of computed joints + values : `list` + values of computed joints + error_codes : `list` + list of error codes + Error codes: + (-1) Foot is too far (unreachable) + (x) Joint x is out of range + ''' + + error_codes = [] + leg_y_dev, upper_leg_height, upper_leg_depth, lower_leg_len, knee_extra_angle, _ = self.NAO_SPECS + sign = -1 if is_left else 1 + + # Then we translate to origin of leg by shifting the y coordinate + ankle_pos3d = np.asarray(ankle_pos3d) + (0,sign*leg_y_dev,0) + + # First we rotate the leg, then we rotate the coordinates to abstract from the rotation + ankle_pos3d = Matrix_3x3().rotate_z_deg(-foot_ori3d[2]).multiply(ankle_pos3d) + + # Use geometric solution to compute knee angle and foot pitch + dist = np.linalg.norm(ankle_pos3d) #dist hip <-> ankle + sq_dist = dist * dist + sq_upper_leg_h = upper_leg_height * upper_leg_height + sq_lower_leg_l = lower_leg_len * lower_leg_len + sq_upper_leg_l = upper_leg_depth * upper_leg_depth + sq_upper_leg_h + upper_leg_len = sqrt(sq_upper_leg_l) + knee = M.acos((sq_upper_leg_l + sq_lower_leg_l - sq_dist)/(2 * upper_leg_len * lower_leg_len)) + knee_extra_angle # Law of cosines + foot = M.acos((sq_lower_leg_l + sq_dist - sq_upper_leg_l)/(2 * lower_leg_len * dist)) # foot perpendicular to vec(origin->ankle_pos) + + # Check if target is reachable + if dist > upper_leg_len + lower_leg_len: + error_codes.append(-1) + + # Knee and foot + knee_angle = pi - knee + foot_pitch = foot - atan(ankle_pos3d[0] / np.linalg.norm(ankle_pos3d[1:3])) + foot_roll = atan(ankle_pos3d[1] / min(-0.05, ankle_pos3d[2])) * -sign # avoid instability of foot roll (not relevant above -0.05m) + + # Raw hip angles if all joints were straightforward + raw_hip_yaw = foot_ori3d[2] + raw_hip_pitch = foot_pitch - knee_angle + raw_hip_roll = -sign * foot_roll + + # Rotate 45deg due to yaw joint orientation, then rotate yaw, roll and pitch + m = Matrix_3x3().rotate_y_rad(raw_hip_pitch).rotate_x_rad(raw_hip_roll).rotate_z_deg(raw_hip_yaw).rotate_x_deg(-45*sign) + + # Get actual hip angles considering the yaw joint orientation + hip_roll = (pi/4) - (sign * asin(m.m[1,2])) #Add pi/4 due to 45deg rotation + hip_pitch = - atan2(m.m[0,2],m.m[2,2]) + hip_yaw = sign * atan2(m.m[1,0],m.m[1,1]) + + # Convert rad to deg + values = np.array([hip_yaw,hip_roll,hip_pitch,-knee_angle,foot_pitch,foot_roll]) * 57.2957795 #rad to deg + + # Set feet rotation bias (based on vertical pose, or dynamic_pose) + values[4] -= foot_ori3d[1] + values[5] -= foot_ori3d[0] * sign + + indices = [2,4,6,8,10,12] if is_left else [3,5,7,9,11,13] + + if dynamic_pose: + + # Rotation of torso in relation to foot + m : Matrix_3x3 = Matrix_3x3.from_rotation_deg((self.robot.imu_torso_roll, self.robot.imu_torso_pitch, 0)) + m.rotate_z_deg(foot_ori3d[2], True) + + roll = m.get_roll_deg() + pitch = m.get_pitch_deg() + + # Simple balance algorithm + correction = 1 #correction to motivate a vertical torso (in degrees) + roll = 0 if abs(roll) < correction else roll - np.copysign(correction,roll) + pitch = 0 if abs(pitch) < correction else pitch - np.copysign(correction,pitch) + + values[4] += pitch + values[5] += roll * sign + + + # Check and limit range of joints + for i in range(len(indices)): + if values[i] < self.robot.joints_info[indices[i]].min or values[i] > self.robot.joints_info[indices[i]].max: + error_codes.append(indices[i]) + values[i] = np.clip(values[i], self.robot.joints_info[indices[i]].min, self.robot.joints_info[indices[i]].max) + + + return indices, values, error_codes + diff --git a/math_ops/Math_Ops.py b/math_ops/Math_Ops.py new file mode 100644 index 0000000..ddbf4f3 --- /dev/null +++ b/math_ops/Math_Ops.py @@ -0,0 +1,361 @@ +from math import acos, asin, atan2, cos, pi, sin, sqrt +import numpy as np +import sys + +try: + GLOBAL_DIR = sys._MEIPASS # temporary folder with libs & data files +except: + GLOBAL_DIR = "." + + +class Math_Ops(): + ''' + This class provides general mathematical operations that are not directly available through numpy + ''' + + @staticmethod + def deg_sph2cart(spherical_vec): + ''' Converts SimSpark's spherical coordinates in degrees to cartesian coordinates ''' + r = spherical_vec[0] + h = spherical_vec[1] * pi / 180 + v = spherical_vec[2] * pi / 180 + return np.array([r * cos(v) * cos(h), r * cos(v) * sin(h), r * sin(v)]) + + @staticmethod + def deg_sin(deg_angle): + ''' Returns sin of degrees ''' + return sin(deg_angle * pi / 180) + + @staticmethod + def deg_cos(deg_angle): + ''' Returns cos of degrees ''' + return cos(deg_angle * pi / 180) + + @staticmethod + def to_3d(vec_2d, value=0) -> np.ndarray: + ''' Returns new 3d vector from 2d vector ''' + return np.append(vec_2d,value) + + @staticmethod + def to_2d_as_3d(vec_3d) -> np.ndarray: + ''' Returns new 3d vector where the 3rd dimension is zero ''' + vec_2d_as_3d = np.copy(vec_3d) + vec_2d_as_3d[2] = 0 + return vec_2d_as_3d + + @staticmethod + def normalize_vec(vec) -> np.ndarray: + ''' Divides vector by its length ''' + size = np.linalg.norm(vec) + if size == 0: return vec + return vec / size + + @staticmethod + def get_active_directory(dir:str) -> str: + global GLOBAL_DIR + return GLOBAL_DIR + dir + + @staticmethod + def acos(val): + ''' arccosine function that limits input ''' + return acos( np.clip(val,-1,1) ) + + @staticmethod + def asin(val): + ''' arcsine function that limits input ''' + return asin( np.clip(val,-1,1) ) + + @staticmethod + def normalize_deg(val): + ''' normalize val in range [-180,180[ ''' + return (val + 180.0) % 360 - 180 + + @staticmethod + def normalize_rad(val): + ''' normalize val in range [-pi,pi[ ''' + return (val + pi) % (2*pi) - pi + + @staticmethod + def deg_to_rad(val): + ''' convert degrees to radians ''' + return val * 0.01745329251994330 + + @staticmethod + def rad_to_deg(val): + ''' convert radians to degrees ''' + return val * 57.29577951308232 + + @staticmethod + def vector_angle(vector, is_rad=False): + ''' angle (degrees or radians) of 2D vector ''' + if is_rad: + return atan2(vector[1], vector[0]) + else: + return atan2(vector[1], vector[0]) * 180 / pi + + @staticmethod + def vectors_angle(vec1, vec2, is_rad=False): + ''' get angle between vectors (degrees or radians) ''' + ang_rad = acos(np.dot(Math_Ops.normalize_vec(vec1),Math_Ops.normalize_vec(vec2))) + return ang_rad if is_rad else ang_rad * 180 / pi + + @staticmethod + def vector_from_angle(angle, is_rad=False): + ''' unit vector with direction given by `angle` ''' + if is_rad: + return np.array([cos(angle), sin(angle)], float) + else: + return np.array([Math_Ops.deg_cos(angle), Math_Ops.deg_sin(angle)], float) + + @staticmethod + def target_abs_angle(pos2d, target, is_rad=False): + ''' angle (degrees or radians) of vector (target-pos2d) ''' + if is_rad: + return atan2(target[1]-pos2d[1], target[0]-pos2d[0]) + else: + return atan2(target[1]-pos2d[1], target[0]-pos2d[0]) * 180 / pi + + @staticmethod + def target_rel_angle(pos2d, ori, target, is_rad=False): + ''' relative angle (degrees or radians) of target if we're located at 'pos2d' with orientation 'ori' (degrees or radians) ''' + if is_rad: + return Math_Ops.normalize_rad( atan2(target[1]-pos2d[1], target[0]-pos2d[0]) - ori ) + else: + return Math_Ops.normalize_deg( atan2(target[1]-pos2d[1], target[0]-pos2d[0]) * 180 / pi - ori ) + + @staticmethod + def rotate_2d_vec(vec, angle, is_rad=False): + ''' rotate 2D vector anticlockwise around the origin by `angle` ''' + cos_ang = cos(angle) if is_rad else cos(angle * pi / 180) + sin_ang = sin(angle) if is_rad else sin(angle * pi / 180) + return np.array([cos_ang*vec[0]-sin_ang*vec[1], sin_ang*vec[0]+cos_ang*vec[1]]) + + @staticmethod + def distance_point_to_line(p:np.ndarray, a:np.ndarray, b:np.ndarray): + ''' + Distance between point p and 2d line 'ab' (and side where p is) + + Parameters + ---------- + a : ndarray + 2D point that defines line + b : ndarray + 2D point that defines line + p : ndarray + 2D point + + Returns + ------- + distance : float + distance between line and point + side : str + if we are at a, looking at b, p may be at our "left" or "right" + ''' + line_len = np.linalg.norm(b-a) + + if line_len == 0: # assumes vertical line + dist = sdist = np.linalg.norm(p-a) + else: + sdist = np.cross(b-a,p-a)/line_len + dist = abs(sdist) + + return dist, "left" if sdist>0 else "right" + + @staticmethod + def distance_point_to_segment(p:np.ndarray, a:np.ndarray, b:np.ndarray): + ''' Distance from point p to 2d line segment 'ab' ''' + + ap = p-a + ab = b-a + + ad = Math_Ops.vector_projection(ap,ab) + + # Is d in ab? We can find k in (ad = k * ab) without computing any norm + # we use the largest dimension of ab to avoid division by 0 + k = ad[0]/ab[0] if abs(ab[0])>abs(ab[1]) else ad[1]/ab[1] + + if k <= 0: return np.linalg.norm(ap) + elif k >= 1: return np.linalg.norm(p-b) + else: return np.linalg.norm(p-(ad + a)) # p-d + + @staticmethod + def distance_point_to_ray(p:np.ndarray, ray_start:np.ndarray, ray_direction:np.ndarray): + ''' Distance from point p to 2d ray ''' + + rp = p-ray_start + rd = Math_Ops.vector_projection(rp,ray_direction) + + # Is d in ray? We can find k in (rd = k * ray_direction) without computing any norm + # we use the largest dimension of ray_direction to avoid division by 0 + k = rd[0]/ray_direction[0] if abs(ray_direction[0])>abs(ray_direction[1]) else rd[1]/ray_direction[1] + + if k <= 0: return np.linalg.norm(rp) + else: return np.linalg.norm(p-(rd + ray_start)) # p-d + + @staticmethod + def closest_point_on_ray_to_point(p:np.ndarray, ray_start:np.ndarray, ray_direction:np.ndarray): + ''' Point on ray closest to point p ''' + + rp = p-ray_start + rd = Math_Ops.vector_projection(rp,ray_direction) + + # Is d in ray? We can find k in (rd = k * ray_direction) without computing any norm + # we use the largest dimension of ray_direction to avoid division by 0 + k = rd[0]/ray_direction[0] if abs(ray_direction[0])>abs(ray_direction[1]) else rd[1]/ray_direction[1] + + if k <= 0: return ray_start + else: return rd + ray_start + + @staticmethod + def does_circle_intersect_segment(p:np.ndarray, r, a:np.ndarray, b:np.ndarray): + ''' Returns true if circle (center p, radius r) intersect 2d line segment ''' + + ap = p-a + ab = b-a + + ad = Math_Ops.vector_projection(ap,ab) + + # Is d in ab? We can find k in (ad = k * ab) without computing any norm + # we use the largest dimension of ab to avoid division by 0 + k = ad[0]/ab[0] if abs(ab[0])>abs(ab[1]) else ad[1]/ab[1] + + if k <= 0: return np.dot(ap,ap) <= r*r + elif k >= 1: return np.dot(p-b,p-b) <= r*r + + dp = p-(ad + a) + return np.dot(dp,dp) <= r*r + + @staticmethod + def vector_projection(a:np.ndarray, b:np.ndarray): + ''' Vector projection of a onto b ''' + b_dot = np.dot(b,b) + return b * np.dot(a,b) / b_dot if b_dot != 0 else b + + @staticmethod + def do_noncollinear_segments_intersect(a,b,c,d): + ''' + Check if 2d line segment 'ab' intersects with noncollinear 2d line segment 'cd' + Explanation: https://www.geeksforgeeks.org/check-if-two-given-line-segments-intersect/ + ''' + + ccw = lambda a,b,c: (c[1]-a[1]) * (b[0]-a[0]) > (b[1]-a[1]) * (c[0]-a[0]) + return ccw(a,c,d) != ccw(b,c,d) and ccw(a,b,c) != ccw(a,b,d) + + @staticmethod + def intersection_segment_opp_goal(a:np.ndarray, b:np.ndarray): + ''' Computes the intersection point of 2d segment 'ab' and the opponents' goal (front line) ''' + vec_x = b[0]-a[0] + + # Collinear intersections are not accepted + if vec_x == 0: return None + + k = (15.01-a[0])/vec_x + + # No collision + if k < 0 or k > 1: return None + + intersection_pt = a + (b-a) * k + + if -1.01 <= intersection_pt[1] <= 1.01: + return intersection_pt + else: + return None + + @staticmethod + def intersection_circle_opp_goal(p:np.ndarray, r): + ''' + Computes the intersection segment of circle (center p, radius r) and the opponents' goal (front line) + Only the y coordinates are returned since the x coordinates are always equal to 15 + ''' + + x_dev = abs(15-p[0]) + + if x_dev > r: + return None # no intersection with x=15 + + y_dev = sqrt(r*r - x_dev*x_dev) + + p1 = max(p[1] - y_dev, -1.01) + p2 = min(p[1] + y_dev, 1.01) + + if p1 == p2: + return p1 # return the y coordinate of a single intersection point + elif p2 < p1: + return None # no intersection + else: + return p1, p2 # return the y coordinates of the intersection segment + + + @staticmethod + def distance_point_to_opp_goal(p:np.ndarray): + ''' Distance between point 'p' and the opponents' goal (front line) ''' + + if p[1] < -1.01: + return np.linalg.norm( p-(15,-1.01) ) + elif p[1] > 1.01: + return np.linalg.norm( p-(15, 1.01) ) + else: + return abs(15-p[0]) + + + @staticmethod + def circle_line_segment_intersection(circle_center, circle_radius, pt1, pt2, full_line=True, tangent_tol=1e-9): + """ Find the points at which a circle intersects a line-segment. This can happen at 0, 1, or 2 points. + + :param circle_center: The (x, y) location of the circle center + :param circle_radius: The radius of the circle + :param pt1: The (x, y) location of the first point of the segment + :param pt2: The (x, y) location of the second point of the segment + :param full_line: True to find intersections along full line - not just in the segment. False will just return intersections within the segment. + :param tangent_tol: Numerical tolerance at which we decide the intersections are close enough to consider it a tangent + :return Sequence[Tuple[float, float]]: A list of length 0, 1, or 2, where each element is a point at which the circle intercepts a line segment. + + Note: We follow: http://mathworld.wolfram.com/Circle-LineIntersection.html + """ + + (p1x, p1y), (p2x, p2y), (cx, cy) = pt1, pt2, circle_center + (x1, y1), (x2, y2) = (p1x - cx, p1y - cy), (p2x - cx, p2y - cy) + dx, dy = (x2 - x1), (y2 - y1) + dr = (dx ** 2 + dy ** 2)**.5 + big_d = x1 * y2 - x2 * y1 + discriminant = circle_radius ** 2 * dr ** 2 - big_d ** 2 + + if discriminant < 0: # No intersection between circle and line + return [] + else: # There may be 0, 1, or 2 intersections with the segment + intersections = [ + (cx + (big_d * dy + sign * (-1 if dy < 0 else 1) * dx * discriminant**.5) / dr ** 2, + cy + (-big_d * dx + sign * abs(dy) * discriminant**.5) / dr ** 2) + for sign in ((1, -1) if dy < 0 else (-1, 1))] # This makes sure the order along the segment is correct + if not full_line: # If only considering the segment, filter out intersections that do not fall within the segment + fraction_along_segment = [ + (xi - p1x) / dx if abs(dx) > abs(dy) else (yi - p1y) / dy for xi, yi in intersections] + intersections = [pt for pt, frac in zip( + intersections, fraction_along_segment) if 0 <= frac <= 1] + # If line is tangent to circle, return just one point (as both intersections have same location) + if len(intersections) == 2 and abs(discriminant) <= tangent_tol: + return [intersections[0]] + else: + return intersections + + + + + # adapted from https://stackoverflow.com/questions/3252194/numpy-and-line-intersections + @staticmethod + def get_line_intersection(a1, a2, b1, b2): + """ + Returns the point of intersection of the lines passing through a2,a1 and b2,b1. + a1: [x, y] a point on the first line + a2: [x, y] another point on the first line + b1: [x, y] a point on the second line + b2: [x, y] another point on the second line + """ + s = np.vstack([a1,a2,b1,b2]) # s for stacked + h = np.hstack((s, np.ones((4, 1)))) # h for homogeneous + l1 = np.cross(h[0], h[1]) # get first line + l2 = np.cross(h[2], h[3]) # get second line + x, y, z = np.cross(l1, l2) # point of intersection + if z == 0: # lines are parallel + return np.array([float('inf'), float('inf')]) + return np.array([x/z, y/z],float) diff --git a/math_ops/Matrix_3x3.py b/math_ops/Matrix_3x3.py new file mode 100644 index 0000000..f34ba47 --- /dev/null +++ b/math_ops/Matrix_3x3.py @@ -0,0 +1,350 @@ +from math import asin, atan2, pi, sqrt +import numpy as np + +class Matrix_3x3(): + + def __init__(self, matrix = None) -> None: + ''' + Constructor examples: + a = Matrix_3x3( ) # create identity matrix + b = Matrix_3x3( [[1,1,1],[2,2,2],[3,3,3]] ) # manually initialize matrix + c = Matrix_3x3( [1,1,1,2,2,2,3,3,3] ) # manually initialize matrix + d = Matrix_3x3( b ) # copy constructor + ''' + if matrix is None: + self.m = np.identity(3) + elif type(matrix) == Matrix_3x3: + self.m = np.copy(matrix.m) + else: + self.m = np.asarray(matrix) + self.m.shape = (3,3) #reshape if needed, throw error if impossible + + + self.rotation_shortcuts={(1,0,0):self.rotate_x_rad, (-1, 0, 0):self._rotate_x_neg_rad, + (0,1,0):self.rotate_y_rad, ( 0,-1, 0):self._rotate_y_neg_rad, + (0,0,1):self.rotate_z_rad, ( 0, 0,-1):self._rotate_z_neg_rad} + + @classmethod + def from_rotation_deg(cls, euler_vec): + ''' + Create rotation matrix from Euler angles, in degrees. + Rotation order: RotZ*RotY*RotX + + Parameters + ---------- + euler_vec : array_like, length 3 + vector with Euler angles (x,y,z) aka (roll, pitch, yaw) + + Example + ---------- + Matrix_3x3.from_rotation_deg((roll,pitch,yaw)) # Creates: RotZ(yaw)*RotY(pitch)*RotX(roll) + ''' + mat = cls().rotate_z_deg(euler_vec[2], True).rotate_y_deg(euler_vec[1], True).rotate_x_deg(euler_vec[0], True) + return mat + + def get_roll_deg(self): + ''' Get angle around the x-axis in degrees, Rotation order: RotZ*RotY*RotX=Rot ''' + if self.m[2,1] == 0 and self.m[2,2] == 0: + return 180 + return atan2(self.m[2,1], self.m[2,2]) * 180 / pi + + def get_pitch_deg(self): + ''' Get angle around the y-axis in degrees, Rotation order: RotZ*RotY*RotX=Rot ''' + return atan2(-self.m[2,0], sqrt(self.m[2,1]*self.m[2,1] + self.m[2,2]*self.m[2,2])) * 180 / pi + + def get_yaw_deg(self): + ''' Get angle around the z-axis in degrees, Rotation order: RotZ*RotY*RotX=Rot ''' + if self.m[1,0] == 0 and self.m[0,0] == 0: + return atan2(self.m[0,1], self.m[1,1]) * 180 / pi + return atan2(self.m[1,0], self.m[0,0]) * 180 / pi + + def get_inclination_deg(self): + ''' Get inclination of z-axis in relation to reference z-axis ''' + return 90 - (asin(self.m[2,2]) * 180 / pi) + + + def rotate_deg(self, rotation_vec, rotation_deg, in_place=False): + ''' + Rotates the current rotation matrix + + Parameters + ---------- + rotation_vec : array_like, length 3 + rotation vector + rotation_rad : float + rotation in degrees + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_3x3 + self is returned if in_place is True + ''' + return self.rotate_rad(rotation_vec, rotation_deg * (pi/180) , in_place) + + + def rotate_rad(self, rotation_vec, rotation_rad, in_place=False): + ''' + Rotates the current rotation matrix + + Parameters + ---------- + rotation_vec : array_like, length 3 + rotation vector + rotation_rad : float + rotation in radians + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_3x3 + self is returned if in_place is True + ''' + + if rotation_rad == 0: return + + shortcut = self.rotation_shortcuts.get(tuple(a for a in rotation_vec)) + if shortcut: + return shortcut(rotation_rad, in_place) + + c = np.math.cos(rotation_rad) + c1 = 1 - c + s = np.math.sin(rotation_rad) + x = rotation_vec[0] + y = rotation_vec[1] + z = rotation_vec[2] + xxc1 = x * x * c1 + yyc1 = y * y * c1 + zzc1 = z * z * c1 + xyc1 = x * y * c1 + xzc1 = x * z * c1 + yzc1 = y * z * c1 + xs = x * s + ys = y * s + zs = z * s + + mat = np.array([ + [xxc1 + c, xyc1 - zs, xzc1 + ys], + [xyc1 + zs, yyc1 + c, yzc1 - xs], + [xzc1 - ys, yzc1 + xs, zzc1 + c]]) + + return self.multiply(mat, in_place) + + + def _rotate_x_neg_rad(self, rotation_rad, in_place=False): + self.rotate_x_rad(-rotation_rad, in_place) + + def _rotate_y_neg_rad(self, rotation_rad, in_place=False): + self.rotate_y_rad(-rotation_rad, in_place) + + def _rotate_z_neg_rad(self, rotation_rad, in_place=False): + self.rotate_z_rad(-rotation_rad, in_place) + + def rotate_x_rad(self, rotation_rad, in_place=False): + ''' + Rotates the current rotation matrix around the x-axis + + Parameters + ---------- + rotation_rad : float + rotation in radians + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_3x3 + self is returned if in_place is True + ''' + if rotation_rad == 0: + return self if in_place else Matrix_3x3(self) + + c = np.math.cos(rotation_rad) + s = np.math.sin(rotation_rad) + + mat = np.array([ + [1, 0, 0], + [0, c,-s], + [0, s, c]]) + + return self.multiply(mat, in_place) + + def rotate_y_rad(self, rotation_rad, in_place=False): + ''' + Rotates the current rotation matrix around the y-axis + + Parameters + ---------- + rotation_rad : float + rotation in radians + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_3x3 + self is returned if in_place is True + ''' + if rotation_rad == 0: + return self if in_place else Matrix_3x3(self) + + c = np.math.cos(rotation_rad) + s = np.math.sin(rotation_rad) + + mat = np.array([ + [ c, 0, s], + [ 0, 1, 0], + [-s, 0, c]]) + + return self.multiply(mat, in_place) + + def rotate_z_rad(self, rotation_rad, in_place=False): + ''' + Rotates the current rotation matrix around the z-axis + + Parameters + ---------- + rotation_rad : float + rotation in radians + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_3x3 + self is returned if in_place is True + ''' + if rotation_rad == 0: + return self if in_place else Matrix_3x3(self) + + c = np.math.cos(rotation_rad) + s = np.math.sin(rotation_rad) + + mat = np.array([ + [ c,-s, 0], + [ s, c, 0], + [ 0, 0, 1]]) + + return self.multiply(mat, in_place) + + def rotate_x_deg(self, rotation_deg, in_place=False): + ''' + Rotates the current rotation matrix around the x-axis + + Parameters + ---------- + rotation_rad : float + rotation in degrees + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_3x3 + self is returned if in_place is True + ''' + return self.rotate_x_rad(rotation_deg * (pi/180), in_place) + + def rotate_y_deg(self, rotation_deg, in_place=False): + ''' + Rotates the current rotation matrix around the y-axis + + Parameters + ---------- + rotation_rad : float + rotation in degrees + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_3x3 + self is returned if in_place is True + ''' + return self.rotate_y_rad(rotation_deg * (pi/180), in_place) + + def rotate_z_deg(self, rotation_deg, in_place=False): + ''' + Rotates the current rotation matrix around the z-axis + + Parameters + ---------- + rotation_rad : float + rotation in degrees + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_3x3 + self is returned if in_place is True + ''' + return self.rotate_z_rad(rotation_deg * (pi/180), in_place) + + def invert(self, in_place=False): + ''' + Inverts the current rotation matrix + + Parameters + ---------- + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_3x3 + self is returned if in_place is True + ''' + + if in_place: + self.m = np.linalg.inv(self.m) + return self + else: + return Matrix_3x3(np.linalg.inv(self.m)) + + def multiply(self,mat, in_place=False, reverse_order=False): + ''' + Multiplies the current rotation matrix by mat + + Parameters + ---------- + mat : Matrix_3x3 or array_like + multiplier matrix or 3D vector + in_place: bool, optional + - True: the internal matrix is changed in-place + - False: a new matrix is returned and the current one is not changed (default) + reverse_order: bool, optional + - False: self * mat + - True: mat * self + + Returns + ------- + result : Matrix_3x3 | array_like + Matrix_3x3 is returned if mat is a matrix (self is returned if in_place is True); + a 3D vector is returned if mat is a vector + ''' + # get array from matrix object or convert to numpy array (if needed) + mat = mat.m if type(mat) == Matrix_3x3 else np.asarray(mat) + + a,b = (mat, self.m) if reverse_order else (self.m, mat) + + if mat.ndim == 1: + return np.matmul(a, b) # multiplication by 3D vector + elif in_place: + np.matmul(a, b, self.m) # multiplication by matrix, in place + return self + else: # multiplication by matrix, return new Matrix_3x3 + return Matrix_3x3(np.matmul(a, b)) + + diff --git a/math_ops/Matrix_4x4.py b/math_ops/Matrix_4x4.py new file mode 100644 index 0000000..ec11b49 --- /dev/null +++ b/math_ops/Matrix_4x4.py @@ -0,0 +1,440 @@ +from math import asin, atan2, pi, sqrt +from math_ops.Math_Ops import Math_Ops as M +from math_ops.Matrix_3x3 import Matrix_3x3 +import numpy as np + +class Matrix_4x4(): + + def __init__(self, matrix = None) -> None: + ''' + Constructor examples: + a = Matrix_4x4( ) # create identity matrix + b = Matrix_4x4( [[1,1,1,1],[2,2,2,2],[3,3,3,3],[4,4,4,4]] ) # manually initialize matrix + c = Matrix_4x4( [1,1,1,1,2,2,2,2,3,3,3,3,4,4,4,4] ) # manually initialize matrix + d = Matrix_4x4( b ) # copy constructor + ''' + if matrix is None: + self.m = np.identity(4) + elif type(matrix) == Matrix_4x4: + self.m = np.copy(matrix.m) + elif type(matrix) == Matrix_3x3: + self.m = np.identity(4) + self.m[0:3,0:3] = matrix.m + else: + self.m = np.asarray(matrix) + self.m.shape = (4,4) #reshape if needed, throw error if impossible + + + @classmethod + def from_translation(cls, translation_vec): + ''' + Create transformation matrix from translation_vec translation + e.g. Matrix_4x4.from_translation((a,b,c)) + output: [[1,0,0,a],[0,1,0,b],[0,0,1,c],[0,0,0,1]] + ''' + mat = np.identity(4) + mat[0:3,3] = translation_vec + return cls(mat) + + @classmethod + def from_3x3_and_translation(cls, mat3x3:Matrix_3x3, translation_vec): + ''' + Create transformation matrix from rotation matrix (3x3) and translation + e.g. Matrix_4x4.from_3x3_and_translation(r,(a,b,c)) + output: [[r00,r01,r02,a],[r10,r11,r12,b],[r20,r21,r22,c],[0,0,0,1]] + ''' + mat = np.identity(4) + mat[0:3,0:3] = mat3x3.m + mat[0:3,3] = translation_vec + return cls(mat) + + def translate(self, translation_vec, in_place=False): + ''' + Translates the current transformation matrix + + Parameters + ---------- + translation_vec : array_like, length 3 + translation vector + in_place: bool, optional + * True: the internal matrix is changed in-place + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_4x4 + self is returned if in_place is True + ''' + vec = np.array([*translation_vec,1])# conversion to 4D vector + np.matmul(self.m, vec, out=vec) # compute only 4th column + + if in_place: + self.m[:,3] = vec + return self + else: + ret = Matrix_4x4(self.m) + ret.m[:,3] = vec + return ret + + + def get_translation(self): + ''' Get translation vector (x,y,z) ''' + return self.m[0:3,3] # return view + + def get_x(self): + return self.m[0,3] + + def get_y(self): + return self.m[1,3] + + def get_z(self): + return self.m[2,3] + + def get_rotation_4x4(self): + ''' Get Matrix_4x4 without translation ''' + mat = Matrix_4x4(self) + mat.m[0:3,3] = 0 + return mat + + def get_rotation(self): + ''' Get rotation Matrix_3x3 ''' + return Matrix_3x3(self.m[0:3,0:3]) + + def get_distance(self): + ''' Get translation vector length ''' + return np.linalg.norm(self.m[0:3,3]) + + def get_roll_deg(self): + ''' Get angle around the x-axis in degrees, Rotation order: RotZ*RotY*RotX=Rot ''' + if self.m[2,1] == 0 and self.m[2,2] == 0: + return 180 + return atan2(self.m[2,1], self.m[2,2]) * 180 / pi + + def get_pitch_deg(self): + ''' Get angle around the y-axis in degrees, Rotation order: RotZ*RotY*RotX=Rot ''' + return atan2(-self.m[2,0], sqrt(self.m[2,1]*self.m[2,1] + self.m[2,2]*self.m[2,2])) * 180 / pi + + def get_yaw_deg(self): + ''' Get angle around the z-axis in degrees, Rotation order: RotZ*RotY*RotX=Rot ''' + if self.m[1,0] == 0 and self.m[0,0] == 0: + return atan2(self.m[0,1], self.m[1,1]) * 180 / pi + return atan2(self.m[1,0], self.m[0,0]) * 180 / pi + + def get_inclination_deg(self): + ''' Get inclination of z-axis in relation to reference z-axis ''' + return 90 - (asin(np.clip(self.m[2,2],-1,1)) * 180 / pi) + + def rotate_deg(self, rotation_vec, rotation_deg, in_place=False): + ''' + Rotates the current transformation matrix + + Parameters + ---------- + rotation_vec : array_like, length 3 + rotation vector + rotation_rad : float + rotation in degrees + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_4x4 + self is returned if in_place is True + ''' + return self.rotate_rad(rotation_vec, rotation_deg * (pi/180) , in_place) + + + def rotate_rad(self, rotation_vec, rotation_rad, in_place=False): + ''' + Rotates the current transformation matrix + + Parameters + ---------- + rotation_vec : array_like, length 3 + rotation vector + rotation_rad : float + rotation in radians + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_4x4 + self is returned if in_place is True + ''' + + if rotation_rad == 0: + return self if in_place else Matrix_4x4(self) + + # shortcuts for rotation around 1 axis + if rotation_vec[0]==0: + if rotation_vec[1]==0: + if rotation_vec[2]==1: + return self.rotate_z_rad(rotation_rad, in_place) + elif rotation_vec[2]==-1: + return self.rotate_z_rad(-rotation_rad, in_place) + elif rotation_vec[2]==0: + if rotation_vec[1]==1: + return self.rotate_y_rad(rotation_rad, in_place) + elif rotation_vec[1]==-1: + return self.rotate_y_rad(-rotation_rad, in_place) + elif rotation_vec[1]==0 and rotation_vec[2]==0: + if rotation_vec[0]==1: + return self.rotate_x_rad(rotation_rad, in_place) + elif rotation_vec[0]==-1: + return self.rotate_x_rad(-rotation_rad, in_place) + + c = np.math.cos(rotation_rad) + c1 = 1 - c + s = np.math.sin(rotation_rad) + x = rotation_vec[0] + y = rotation_vec[1] + z = rotation_vec[2] + xxc1 = x * x * c1 + yyc1 = y * y * c1 + zzc1 = z * z * c1 + xyc1 = x * y * c1 + xzc1 = x * z * c1 + yzc1 = y * z * c1 + xs = x * s + ys = y * s + zs = z * s + + mat = np.array([ + [xxc1 + c, xyc1 - zs, xzc1 + ys, 0], + [xyc1 + zs, yyc1 + c, yzc1 - xs, 0], + [xzc1 - ys, yzc1 + xs, zzc1 + c, 0], + [0, 0, 0, 1]]) + + return self.multiply(mat, in_place) + + + def rotate_x_rad(self, rotation_rad, in_place=False): + ''' + Rotates the current transformation matrix around the x-axis + + Parameters + ---------- + rotation_rad : float + rotation in radians + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_4x4 + self is returned if in_place is True + ''' + if rotation_rad == 0: + return self if in_place else Matrix_4x4(self) + + c = np.math.cos(rotation_rad) + s = np.math.sin(rotation_rad) + + mat = np.array([ + [1, 0, 0, 0], + [0, c,-s, 0], + [0, s, c, 0], + [0, 0, 0, 1]]) + + return self.multiply(mat, in_place) + + def rotate_y_rad(self, rotation_rad, in_place=False): + ''' + Rotates the current transformation matrix around the y-axis + + Parameters + ---------- + rotation_rad : float + rotation in radians + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_4x4 + self is returned if in_place is True + ''' + if rotation_rad == 0: + return self if in_place else Matrix_4x4(self) + + c = np.math.cos(rotation_rad) + s = np.math.sin(rotation_rad) + + mat = np.array([ + [ c, 0, s, 0], + [ 0, 1, 0, 0], + [-s, 0, c, 0], + [ 0, 0, 0, 1]]) + + return self.multiply(mat, in_place) + + def rotate_z_rad(self, rotation_rad, in_place=False): + ''' + Rotates the current transformation matrix around the z-axis + + Parameters + ---------- + rotation_rad : float + rotation in radians + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_4x4 + self is returned if in_place is True + ''' + if rotation_rad == 0: + return self if in_place else Matrix_4x4(self) + + c = np.math.cos(rotation_rad) + s = np.math.sin(rotation_rad) + + mat = np.array([ + [ c,-s, 0, 0], + [ s, c, 0, 0], + [ 0, 0, 1, 0], + [ 0, 0, 0, 1]]) + + return self.multiply(mat, in_place) + + def rotate_x_deg(self, rotation_deg, in_place=False): + ''' + Rotates the current transformation matrix around the x-axis + + Parameters + ---------- + rotation_rad : float + rotation in degrees + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_4x4 + self is returned if in_place is True + ''' + return self.rotate_x_rad(rotation_deg * (pi/180), in_place) + + def rotate_y_deg(self, rotation_deg, in_place=False): + ''' + Rotates the current transformation matrix around the y-axis + + Parameters + ---------- + rotation_rad : float + rotation in degrees + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_4x4 + self is returned if in_place is True + ''' + return self.rotate_y_rad(rotation_deg * (pi/180), in_place) + + def rotate_z_deg(self, rotation_deg, in_place=False): + ''' + Rotates the current transformation matrix around the z-axis + + Parameters + ---------- + rotation_rad : float + rotation in degrees + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_4x4 + self is returned if in_place is True + ''' + return self.rotate_z_rad(rotation_deg * (pi/180), in_place) + + def invert(self, in_place=False): + ''' + Inverts the current transformation matrix + + Parameters + ---------- + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed + + Returns + ------- + result : Matrix_4x4 + self is returned if in_place is True + ''' + + if in_place: + self.m = np.linalg.inv(self.m) + return self + else: + return Matrix_4x4(np.linalg.inv(self.m)) + + def multiply(self,mat, in_place=False): + ''' + Multiplies the current transformation matrix by mat + + Parameters + ---------- + mat : Matrix_4x4 or array_like + multiplier matrix or 3D vector + in_place: bool, optional + * True: the internal matrix is changed in-place (default) + * False: a new matrix is returned and the current one is not changed (if mat is a 4x4 matrix) + + Returns + ------- + result : Matrix_4x4 | array_like + Matrix_4x4 is returned if mat is a matrix (self is returned if in_place is True); + a 3D vector is returned if mat is a vector + ''' + if type(mat) == Matrix_4x4: + mat = mat.m + else: + mat = np.asarray(mat) # conversion to array, if needed + if mat.ndim == 1: # multiplication by 3D vector + vec = np.append(mat,1) # conversion to 4D vector + return np.matmul(self.m, vec)[0:3] # conversion to 3D vector + + if in_place: + np.matmul(self.m, mat, self.m) + return self + else: + return Matrix_4x4(np.matmul(self.m, mat)) + + def __call__(self,mat, is_spherical=False): + ''' + Multiplies the current transformation matrix by mat and returns a new matrix or vector + + Parameters + ---------- + mat : Matrix_4x4 or array_like + multiplier matrix or 3D vector + is_spherical : bool + only relevant if mat is a 3D vector, True if it uses spherical coordinates + + Returns + ------- + result : Matrix_4x4 | array_like + Matrix_4x4 is returned if mat is a matrix; + a 3D vector is returned if mat is a vector + ''' + + if is_spherical and mat.ndim == 1: mat = M.deg_sph2cart(mat) + return self.multiply(mat,False) + + \ No newline at end of file diff --git a/math_ops/Neural_Network.py b/math_ops/Neural_Network.py new file mode 100644 index 0000000..fec7016 --- /dev/null +++ b/math_ops/Neural_Network.py @@ -0,0 +1,28 @@ +import numpy as np + + +def run_mlp(obs, weights, activation_function="tanh"): + ''' + Run multilayer perceptron using numpy + + Parameters + ---------- + obs : ndarray + float32 array with neural network inputs + weights : list + list of MLP layers of type (bias, kernel) + activation_function : str + activation function for hidden layers + set to "none" to disable + ''' + + obs = obs.astype(np.float32, copy=False) + out = obs + + for w in weights[:-1]: # for each hidden layer + out = np.matmul(w[1],out) + w[0] + if activation_function == "tanh": + np.tanh(out, out=out) + elif activation_function != "none": + raise NotImplementedError + return np.matmul(weights[-1][1],out) + weights[-1][0] # final layer \ No newline at end of file diff --git a/scripts/commons/Script.py b/scripts/commons/Script.py new file mode 100644 index 0000000..d14ebed --- /dev/null +++ b/scripts/commons/Script.py @@ -0,0 +1,300 @@ +from os import path, listdir, getcwd, cpu_count +from os.path import join, realpath, dirname, isfile, isdir, getmtime +from scripts.commons.UI import UI +import __main__ +import argparse,json,sys +import pickle +import subprocess + + +class Script(): + ROOT_DIR = path.dirname(path.dirname(realpath( join(getcwd(), dirname(__file__))) )) # project root directory + + def __init__(self, cpp_builder_unum=0) -> None: + + ''' + Arguments specification + ----------------------- + - To add new arguments, edit the information below + - After changing information below, the config.json file must be manually deleted + - In other modules, these arguments can be accessed by their 1-letter ID + ''' + # list of arguments: 1-letter ID, Description, Hardcoded default + self.options = {'i': ('Server Hostname/IP', 'localhost'), + 'p': ('Agent Port', '3100'), + 'm': ('Monitor Port', '3200'), + 't': ('Team Name', 'FCPortugal'), + 'u': ('Uniform Number', '1'), + 'r': ('Robot Type', '1'), + 'P': ('Penalty Shootout', '0'), + 'F': ('magmaFatProxy', '0'), + 'D': ('Debug Mode', '1')} + + # list of arguments: 1-letter ID, data type, choices + self.op_types = {'i': (str, None), + 'p': (int, None), + 'm': (int, None), + 't': (str, None), + 'u': (int, range(1,12)), + 'r': (int, [0,1,2,3,4]), + 'P': (int, [0,1]), + 'F': (int, [0,1]), + 'D': (int, [0,1])} + + ''' + End of arguments specification + ''' + + self.read_or_create_config() + + #advance help text position + formatter = lambda prog: argparse.HelpFormatter(prog,max_help_position=52) + parser = argparse.ArgumentParser(formatter_class=formatter) + + o = self.options + t = self.op_types + + for id in self.options: # shorter metavar for aesthetic reasons + parser.add_argument(f"-{id}", help=f"{o[id][0]:30}[{o[id][1]:20}]", type=t[id][0], nargs='?', default=o[id][1], metavar='X', choices=t[id][1]) + + self.args = parser.parse_args() + + if getattr(sys, 'frozen', False): # disable debug mode when running from binary + self.args.D = 0 + + self.players = [] # list of created players + + Script.build_cpp_modules(exit_on_build = (cpp_builder_unum != 0 and cpp_builder_unum != self.args.u)) + + if self.args.D: + try: + print(f"\nNOTE: for help run \"python {__main__.__file__} -h\"") + except: + pass + + columns = [[],[],[]] + for key, value in vars(self.args).items(): + columns[0].append(o[key][0]) + columns[1].append(o[key][1]) + columns[2].append(value) + + UI.print_table(columns, ["Argument","Default at /config.json","Active"], alignment=["<","^","^"]) + + + def read_or_create_config(self) -> None: + + if not path.isfile('config.json'): # Save hardcoded default values if file does not exist + with open("config.json", "w") as f: + json.dump(self.options, f, indent=4) + else: # Load user-defined values (that can be overwritten in the terminal) + with open("config.json", "r") as f: + self.options = json.loads(f.read()) + + + @staticmethod + def build_cpp_modules(special_environment_prefix=[], exit_on_build=False): + ''' + Build C++ modules in folder /cpp using Pybind11 + + Parameters + ---------- + special_environment_prefix : `list` + command prefix to run a given command in the desired environment + useful to compile C++ modules for different python interpreter versions (other than default version) + Conda Env. example: ['conda', 'run', '-n', 'myEnv'] + If [] the default python interpreter is used as compilation target + exit_on_build : bool + exit if there is something to build (so that only 1 player per team builds c++ modules) + ''' + cpp_path = Script.ROOT_DIR + "/cpp/" + exclusions = ["__pycache__"] + + cpp_modules = [d for d in listdir(cpp_path) if isdir(join(cpp_path, d)) and d not in exclusions] + + if not cpp_modules: return #no modules to build + + python_cmd = f"python{sys.version_info.major}.{sys.version_info.minor}" # "python3" can select the wrong version, this prevents that + + def init(): + print("--------------------------\nC++ modules:",cpp_modules) + + try: + process = subprocess.Popen(special_environment_prefix+[python_cmd, "-m", "pybind11", "--includes"], stdout=subprocess.PIPE) + (includes, err) = process.communicate() + process.wait() + except: + print(f"Error while executing child program: '{python_cmd} -m pybind11 --includes'") + exit() + + includes = includes.decode().rstrip() # strip trailing newlines (and other whitespace chars) + print("Using Pybind11 includes: '",includes,"'",sep="") + return includes + + nproc = str(cpu_count()) + zero_modules = True + + for module in cpp_modules: + module_path = join(cpp_path, module) + + # skip module if there is no Makefile (typical distribution case) + if not isfile(join(module_path, "Makefile")): + continue + + # skip module in certain conditions + if isfile(join(module_path, module+".so")) and isfile(join(module_path, module+".c_info")): + with open(join(module_path, module+".c_info"), 'rb') as f: + info = pickle.load(f) + if info == python_cmd: + code_mod_time = max(getmtime(join(module_path, f)) for f in listdir(module_path) if f.endswith(".cpp") or f.endswith(".h")) + bin_mod_time = getmtime(join(module_path, module+".so")) + if bin_mod_time + 30 > code_mod_time: # favor not building with a margin of 30s (scenario: we unzip the fcpy project, including the binaries, the modification times are all similar) + continue + + # init: print stuff & get Pybind11 includes + if zero_modules: + if exit_on_build: + print("There are C++ modules to build. This player is not allowed to build. Aborting.") + exit() + zero_modules = False + includes = init() + + # build module + print(f'{f"Building: {module}... ":40}',end='',flush=True) + process = subprocess.Popen(['make', '-j'+nproc, 'PYBIND_INCLUDES='+includes], stdout=subprocess.PIPE, stderr=subprocess.PIPE, cwd=module_path) + (output, err) = process.communicate() + exit_code = process.wait() + if exit_code == 0: + print("success!") + with open(join(module_path, module+".c_info"),"wb") as f: # save python version + pickle.dump(python_cmd, f, protocol=4) # protocol 4 is backward compatible with Python 3.4 + else: + print("Aborting! Building errors:") + print(output.decode(), err.decode()) + exit() + + if not zero_modules: + print("All modules were built successfully!\n--------------------------") + + + def batch_create(self, agent_cls, args_per_player): + ''' Creates batch of agents ''' + + for a in args_per_player: + self.players.append( agent_cls(*a) ) + + def batch_execute_agent(self, index : slice = slice(None)): + ''' + Executes agent normally (including commit & send) + + Parameters + ---------- + index : slice + subset of agents + (e.g. index=slice(1,2) will select the second agent) + (e.g. index=slice(1,3) will select the second and third agents) + by default, all agents are selected + ''' + for p in self.players[index]: + p.think_and_send() + + def batch_execute_behavior(self, behavior, index : slice = slice(None)): + ''' + Executes behavior + + Parameters + ---------- + behavior : str + name of behavior to execute + index : slice + subset of agents + (e.g. index=slice(1,2) will select the second agent) + (e.g. index=slice(1,3) will select the second and third agents) + by default, all agents are selected + ''' + for p in self.players[index]: + p.behavior.execute(behavior) + + def batch_commit_and_send(self, index : slice = slice(None)): + ''' + Commits & sends data to server + + Parameters + ---------- + index : slice + subset of agents + (e.g. index=slice(1,2) will select the second agent) + (e.g. index=slice(1,3) will select the second and third agents) + by default, all agents are selected + ''' + for p in self.players[index]: + p.scom.commit_and_send( p.world.robot.get_command() ) + + def batch_receive(self, index : slice = slice(None), update=True): + ''' + Waits for server messages + + Parameters + ---------- + index : slice + subset of agents + (e.g. index=slice(1,2) will select the second agent) + (e.g. index=slice(1,3) will select the second and third agents) + by default, all agents are selected + update : bool + update world state based on information received from server + if False, the agent becomes unaware of itself and its surroundings + which is useful for reducing cpu resources for dummy agents in demonstrations + ''' + for p in self.players[index]: + p.scom.receive(update) + + def batch_commit_beam(self, pos2d_and_rotation, index : slice = slice(None)): + ''' + Beam all player to 2D position with a given rotation + + Parameters + ---------- + pos2d_and_rotation : `list` + iterable of 2D positions and rotations e.g. [(0,0,45),(-5,0,90)] + index : slice + subset of agents + (e.g. index=slice(1,2) will select the second agent) + (e.g. index=slice(1,3) will select the second and third agents) + by default, all agents are selected + ''' + for p, pos_rot in zip(self.players[index], pos2d_and_rotation): + p.scom.commit_beam(pos_rot[0:2],pos_rot[2]) + + def batch_unofficial_beam(self, pos3d_and_rotation, index : slice = slice(None)): + ''' + Beam all player to 3D position with a given rotation + + Parameters + ---------- + pos3d_and_rotation : `list` + iterable of 3D positions and rotations e.g. [(0,0,0.5,45),(-5,0,0.5,90)] + index : slice + subset of agents + (e.g. index=slice(1,2) will select the second agent) + (e.g. index=slice(1,3) will select the second and third agents) + by default, all agents are selected + ''' + for p, pos_rot in zip(self.players[index], pos3d_and_rotation): + p.scom.unofficial_beam(pos_rot[0:3],pos_rot[3]) + + def batch_terminate(self, index : slice = slice(None)): + ''' + Close all sockets connected to the agent port + For scripts where the agent lives until the application ends, this is not needed + + Parameters + ---------- + index : slice + subset of agents + (e.g. index=slice(1,2) will select the second agent) + (e.g. index=slice(1,3) will select the second and third agents) + by default, all agents are selected + ''' + for p in self.players[index]: + p.terminate() + del self.players[index] # delete selection \ No newline at end of file diff --git a/scripts/commons/Server.py b/scripts/commons/Server.py new file mode 100644 index 0000000..caecb28 --- /dev/null +++ b/scripts/commons/Server.py @@ -0,0 +1,60 @@ +import subprocess + +class Server(): + def __init__(self, first_server_p, first_monitor_p, n_servers) -> None: + try: + import psutil + self.check_running_servers(psutil, first_server_p, first_monitor_p, n_servers) + except ModuleNotFoundError: + print("Info: Cannot check if the server is already running, because the psutil module was not found") + + self.first_server_p = first_server_p + self.n_servers = n_servers + self.rcss_processes = [] + + # makes it easier to kill test servers without affecting train servers + cmd = "simspark" if n_servers == 1 else "rcssserver3d" + for i in range(n_servers): + self.rcss_processes.append( + subprocess.Popen((f"{cmd} --agent-port {first_server_p+i} --server-port {first_monitor_p+i}").split(), + stdout=subprocess.DEVNULL, stderr=subprocess.STDOUT, start_new_session=True) + ) + + def check_running_servers(self, psutil, first_server_p, first_monitor_p, n_servers): + ''' Check if any server is running on chosen ports ''' + found = False + p_list = [p for p in psutil.process_iter() if p.cmdline() and p.name() in ["rcssserver3d","simspark"]] + range1 = (first_server_p, first_server_p + n_servers) + range2 = (first_monitor_p,first_monitor_p + n_servers) + bad_processes = [] + + for p in p_list: + # currently ignoring remaining default port when only one of the ports is specified (uncommon scenario) + ports = [int(arg) for arg in p.cmdline()[1:] if arg.isdigit()] + if len(ports) == 0: + ports = [3100,3200] # default server ports (changing this is unlikely) + + conflicts = [str(port) for port in ports if ( + (range1[0] <= port < range1[1]) or (range2[0] <= port < range2[1]) )] + + if len(conflicts)>0: + if not found: + print("\nThere are already servers running on the same port(s)!") + found = True + bad_processes.append(p) + print(f"Port(s) {','.join(conflicts)} already in use by \"{' '.join(p.cmdline())}\" (PID:{p.pid})") + + if found: + print() + while True: + inp = input("Enter 'kill' to kill these processes or ctrl+c to abort. ") + if inp == "kill": + for p in bad_processes: + p.kill() + return + + + def kill(self): + for p in self.rcss_processes: + p.kill() + print(f"Killed {self.n_servers} rcssserver3d processes starting at {self.first_server_p}") diff --git a/scripts/commons/Train_Base.py b/scripts/commons/Train_Base.py new file mode 100644 index 0000000..960b4c8 --- /dev/null +++ b/scripts/commons/Train_Base.py @@ -0,0 +1,494 @@ +from datetime import datetime, timedelta +from itertools import count +from os import listdir +from os.path import isdir, join, isfile +from scripts.commons.UI import UI +from shutil import copy +from stable_baselines3 import PPO +from stable_baselines3.common.base_class import BaseAlgorithm +from stable_baselines3.common.callbacks import EvalCallback, CheckpointCallback, CallbackList, BaseCallback +from typing import Callable +from world.World import World +from xml.dom import minidom +import numpy as np +import os, time, math, csv, select, sys +import pickle +import xml.etree.ElementTree as ET + + +class Train_Base(): + def __init__(self, script) -> None: + ''' + When training with multiple environments (multiprocessing): + The server port is incremented as follows: + self.server_p, self.server_p+1, self.server_p+2, ... + We add +1000 to the initial monitor port, so than we can have more than 100 environments: + self.monitor_p+1000, self.monitor_p+1001, self.monitor_p+1002, ... + When testing we use self.server_p and self.monitor_p + ''' + + args = script.args + self.script = script + self.ip = args.i + self.server_p = args.p # (initial) server port + self.monitor_p = args.m # monitor port when testing + self.monitor_p_1000 = args.m + 1000 # initial monitor port when training + self.robot_type = args.r + self.team = args.t + self.uniform = args.u + self.cf_last_time = 0 + self.cf_delay = 0 + self.cf_target_period = World.STEPTIME # target simulation speed while testing (default: real-time) + + @staticmethod + def prompt_user_for_model(): + + gyms_logs_path = "./scripts/gyms/logs/" + folders = [f for f in listdir(gyms_logs_path) if isdir(join(gyms_logs_path, f))] + folders.sort(key=lambda f: os.path.getmtime(join(gyms_logs_path, f)), reverse=True) # sort by modification date + + while True: + try: + folder_name = UI.print_list(folders,prompt="Choose folder (ctrl+c to return): ")[1] + except KeyboardInterrupt: + print() + return None # ctrl+c + + folder_dir = os.path.join(gyms_logs_path, folder_name) + models = [m[:-4] for m in listdir(folder_dir) if isfile(join(folder_dir, m)) and m.endswith(".zip")] + + if not models: + print("The chosen folder does not contain any .zip file!") + continue + + models.sort(key=lambda m: os.path.getmtime(join(folder_dir, m+".zip")), reverse=True) # sort by modification date + + try: + model_name = UI.print_list(models,prompt="Choose model (ctrl+c to return): ")[1] + break + except KeyboardInterrupt: + print() + + return {"folder_dir":folder_dir, "folder_name":folder_name, "model_file":os.path.join(folder_dir, model_name+".zip")} + + + def control_fps(self, read_input = False): + ''' Add delay to control simulation speed ''' + + if read_input: + speed = input() + if speed == '': + self.cf_target_period = 0 + print(f"Changed simulation speed to MAX") + else: + if speed == '0': + inp = input("Paused. Set new speed or '' to use previous speed:") + if inp != '': + speed = inp + + try: + speed = int(speed) + assert speed >= 0 + self.cf_target_period = World.STEPTIME * 100 / speed + print(f"Changed simulation speed to {speed}%") + except: + print("""Train_Base.py: + Error: To control the simulation speed, enter a non-negative integer. + To disable this control module, use test_model(..., enable_FPS_control=False) in your gym environment.""") + + now = time.time() + period = now - self.cf_last_time + self.cf_last_time = now + self.cf_delay += (self.cf_target_period - period)*0.9 + if self.cf_delay > 0: + time.sleep(self.cf_delay) + else: + self.cf_delay = 0 + + + def test_model(self, model:BaseAlgorithm, env, log_path:str=None, model_path:str=None, max_episodes=0, enable_FPS_control=True, verbose=1): + ''' + Test model and log results + + Parameters + ---------- + model : BaseAlgorithm + Trained model + env : Env + Gym-like environment + log_path : str + Folder where statistics file is saved, default is `None` (no file is saved) + model_path : str + Folder where it reads evaluations.npz to plot it and create evaluations.csv, default is `None` (no plot, no csv) + max_episodes : int + Run tests for this number of episodes + Default is 0 (run until user aborts) + verbose : int + 0 - no output (except if enable_FPS_control=True) + 1 - print episode statistics + ''' + + if model_path is not None: + assert os.path.isdir(model_path), f"{model_path} is not a valid path" + self.display_evaluations(model_path) + + if log_path is not None: + assert os.path.isdir(log_path), f"{log_path} is not a valid path" + + # If file already exists, don't overwrite + if os.path.isfile(log_path + "/test.csv"): + for i in range(1000): + p = f"{log_path}/test_{i:03}.csv" + if not os.path.isfile(p): + log_path = p + break + else: + log_path += "/test.csv" + + with open(log_path, 'w') as f: + f.write("reward,ep. length,rew. cumulative avg., ep. len. cumulative avg.\n") + print("Train statistics are saved to:", log_path) + + if enable_FPS_control: # control simulation speed (using non blocking user input) + print("\nThe simulation speed can be changed by sending a non-negative integer\n" + "(e.g. '50' sets speed to 50%, '0' pauses the simulation, '' sets speed to MAX)\n") + + ep_reward = 0 + ep_length = 0 + rewards_sum = 0 + reward_min = math.inf + reward_max = -math.inf + ep_lengths_sum = 0 + ep_no = 0 + + obs = env.reset() + while True: + action, _states = model.predict(obs, deterministic=True) + obs, reward, done, info = env.step(action) + ep_reward += reward + ep_length += 1 + + if enable_FPS_control: # control simulation speed (using non blocking user input) + self.control_fps(select.select([sys.stdin], [], [], 0)[0]) + + if done: + obs = env.reset() + rewards_sum += ep_reward + ep_lengths_sum += ep_length + reward_max = max(ep_reward, reward_max) + reward_min = min(ep_reward, reward_min) + ep_no += 1 + avg_ep_lengths = ep_lengths_sum/ep_no + avg_rewards = rewards_sum/ep_no + + if verbose > 0: + print( f"\rEpisode: {ep_no:<3} Ep.Length: {ep_length:<4.0f} Reward: {ep_reward:<6.2f} \n", + end=f"--AVERAGE-- Ep.Length: {avg_ep_lengths:<4.0f} Reward: {avg_rewards:<6.2f} (Min: {reward_min:<6.2f} Max: {reward_max:<6.2f})", flush=True) + + if log_path is not None: + with open(log_path, 'a') as f: + writer = csv.writer(f) + writer.writerow([ep_reward, ep_length, avg_rewards, avg_ep_lengths]) + + if ep_no == max_episodes: + return + + ep_reward = 0 + ep_length = 0 + + def learn_model(self, model:BaseAlgorithm, total_steps:int, path:str, eval_env=None, eval_freq=None, eval_eps=5, save_freq=None, backup_env_file=None, export_name=None): + ''' + Learn Model for a specific number of time steps + + Parameters + ---------- + model : BaseAlgorithm + Model to train + total_steps : int + The total number of samples (env steps) to train on + path : str + Path where the trained model is saved + If the path already exists, an incrementing number suffix is added + eval_env : Env + Environment to periodically test the model + Default is None (no periodical evaluation) + eval_freq : int + Evaluate the agent every X steps + Default is None (no periodical evaluation) + eval_eps : int + Evaluate the agent for X episodes (both eval_env and eval_freq must be defined) + Default is 5 + save_freq : int + Saves model at every X steps + Default is None (no periodical checkpoint) + backup_gym_file : str + Generates backup of environment file in model's folder + Default is None (no backup) + export_name : str + If export_name and save_freq are defined, a model is exported every X steps + Default is None (no export) + + Returns + ------- + model_path : str + Directory where model was actually saved (considering incremental suffix) + + Notes + ----- + If `eval_env` and `eval_freq` were specified: + - The policy will be evaluated in `eval_env` every `eval_freq` steps + - Evaluation results will be saved in `path` and shown at the end of training + - Every time the results improve, the model is saved + ''' + + start = time.time() + start_date = datetime.now().strftime("%d/%m/%Y %H:%M:%S") + + # If path already exists, add suffix to avoid overwriting + if os.path.isdir(path): + for i in count(): + p = path.rstrip("/")+f'_{i:03}/' + if not os.path.isdir(p): + path = p + break + os.makedirs(path) + + # Backup environment file + if backup_env_file is not None: + backup_file = os.path.join(path, os.path.basename(backup_env_file)) + copy(backup_env_file, backup_file) + + evaluate = bool(eval_env is not None and eval_freq is not None) + + # Create evaluation callback + eval_callback = None if not evaluate else EvalCallback(eval_env, n_eval_episodes=eval_eps, eval_freq=eval_freq, log_path=path, + best_model_save_path=path, deterministic=True, render=False) + + # Create custom callback to display evaluations + custom_callback = None if not evaluate else Cyclic_Callback(eval_freq, lambda:self.display_evaluations(path,True)) + + # Create checkpoint callback + checkpoint_callback = None if save_freq is None else CheckpointCallback(save_freq=save_freq, save_path=path, name_prefix="model", verbose=1) + + # Create custom callback to export checkpoint models + export_callback = None if save_freq is None or export_name is None else Export_Callback(save_freq, path, export_name) + + callbacks = CallbackList([c for c in [eval_callback, custom_callback, checkpoint_callback, export_callback] if c is not None]) + + model.learn( total_timesteps=total_steps, callback=callbacks ) + model.save( os.path.join(path, "last_model") ) + + # Display evaluations if they exist + if evaluate: + self.display_evaluations(path) + + # Display timestamps + Model path + end_date = datetime.now().strftime('%d/%m/%Y %H:%M:%S') + duration = timedelta(seconds=int(time.time()-start)) + print(f"Train start: {start_date}") + print(f"Train end: {end_date}") + print(f"Train duration: {duration}") + print(f"Model path: {path}") + + # Append timestamps to backup environment file + if backup_env_file is not None: + with open(backup_file, 'a') as f: + f.write(f"\n# Train start: {start_date}\n") + f.write( f"# Train end: {end_date}\n") + f.write( f"# Train duration: {duration}") + + return path + + def display_evaluations(self, path, save_csv=False): + + eval_npz = os.path.join(path, "evaluations.npz") + + if not os.path.isfile(eval_npz): + return + + console_width = 80 + console_height = 18 + symb_x = "\u2022" + symb_o = "\u007c" + symb_xo = "\u237f" + + with np.load(eval_npz) as data: + time_steps = data["timesteps"] + results_raw = np.mean(data["results"],axis=1) + ep_lengths_raw = np.mean(data["ep_lengths"],axis=1) + sample_no = len(results_raw) + + xvals = np.linspace(0, sample_no-1, 80) + results = np.interp(xvals, range(sample_no), results_raw) + ep_lengths = np.interp(xvals, range(sample_no), ep_lengths_raw) + + results_limits = np.min(results), np.max(results) + ep_lengths_limits = np.min(ep_lengths), np.max(ep_lengths) + + results_discrete = np.digitize(results, np.linspace(results_limits[0]-1e-5, results_limits[1]+1e-5, console_height+1))-1 + ep_lengths_discrete = np.digitize(ep_lengths, np.linspace(0, ep_lengths_limits[1]+1e-5, console_height+1))-1 + + matrix = np.zeros((console_height, console_width, 2), int) + matrix[results_discrete[0] ][0][0] = 1 # draw 1st column + matrix[ep_lengths_discrete[0]][0][1] = 1 # draw 1st column + rng = [[results_discrete[0], results_discrete[0]], [ep_lengths_discrete[0], ep_lengths_discrete[0]]] + + # Create continuous line for both plots + for k in range(2): + for i in range(1,console_width): + x = [results_discrete, ep_lengths_discrete][k][i] + if x > rng[k][1]: + rng[k] = [rng[k][1]+1, x] + elif x < rng[k][0]: + rng[k] = [x, rng[k][0]-1] + else: + rng[k] = [x,x] + for j in range(rng[k][0],rng[k][1]+1): + matrix[j][i][k] = 1 + + print(f'{"-"*console_width}') + for l in reversed(range(console_height)): + for c in range(console_width): + if np.all(matrix[l][c] == 0): print(end=" ") + elif np.all(matrix[l][c] == 1): print(end=symb_xo) + elif matrix[l][c][0] == 1: print(end=symb_x) + else: print(end=symb_o) + print() + print(f'{"-"*console_width}') + print(f"({symb_x})-reward min:{results_limits[0]:11.2f} max:{results_limits[1]:11.2f}") + print(f"({symb_o})-ep. length min:{ep_lengths_limits[0]:11.0f} max:{ep_lengths_limits[1]:11.0f} {time_steps[-1]/1000:15.0f}k steps") + print(f'{"-"*console_width}') + + # save CSV + if save_csv: + eval_csv = os.path.join(path, "evaluations.csv") + with open(eval_csv, 'a+') as f: + writer = csv.writer(f) + if sample_no == 1: + writer.writerow(["time_steps", "reward ep.", "length"]) + writer.writerow([time_steps[-1],results_raw[-1],ep_lengths_raw[-1]]) + + + def generate_slot_behavior(self, path, slots, auto_head:bool, XML_name): + ''' + Function that generates the XML file for the optimized slot behavior, overwriting previous files + ''' + + file = os.path.join( path, XML_name ) + + # create the file structure + auto_head = '1' if auto_head else '0' + EL_behavior = ET.Element('behavior',{'description':'Add description to XML file', "auto_head":auto_head}) + + for i,s in enumerate(slots): + EL_slot = ET.SubElement(EL_behavior, 'slot', {'name':str(i), 'delta':str(s[0]/1000)}) + for j in s[1]: # go through all joint indices + ET.SubElement(EL_slot, 'move', {'id':str(j), 'angle':str(s[2][j])}) + + # create XML file + xml_rough = ET.tostring( EL_behavior, 'utf-8' ) + xml_pretty = minidom.parseString(xml_rough).toprettyxml(indent=" ") + with open(file, "w") as x: + x.write(xml_pretty) + + print(file, "was created!") + + @staticmethod + def linear_schedule(initial_value: float) -> Callable[[float], float]: + ''' + Linear learning rate schedule + + Parameters + ---------- + initial_value : float + Initial learning rate + + Returns + ------- + schedule : Callable[[float], float] + schedule that computes current learning rate depending on remaining progress + ''' + def func(progress_remaining: float) -> float: + ''' + Compute learning rate according to current progress + + Parameters + ---------- + progress_remaining : float + Progress will decrease from 1 (beginning) to 0 + + Returns + ------- + learning_rate : float + Learning rate according to current progress + ''' + return progress_remaining * initial_value + + return func + + @staticmethod + def export_model(input_file, output_file, add_sufix=True): + ''' + Export model weights to binary file + + Parameters + ---------- + input_file : str + Input file, compatible with algorithm + output_file : str + Output file, including directory + add_sufix : bool + If true, a suffix is appended to the file name: output_file + "_{index}.pkl" + ''' + + # If file already exists, don't overwrite + if add_sufix: + for i in count(): + f = f"{output_file}_{i:03}.pkl" + if not os.path.isfile(f): + output_file = f + break + + model = PPO.load(input_file) + weights = model.policy.state_dict() # dictionary containing network layers + + w = lambda name : weights[name].detach().cpu().numpy() # extract weights from policy + + var_list = [] + for i in count(0,2): # add hidden layers (step=2 because that's how SB3 works) + if f"mlp_extractor.policy_net.{i}.bias" not in weights: + break + var_list.append([w(f"mlp_extractor.policy_net.{i}.bias"), w(f"mlp_extractor.policy_net.{i}.weight"), "tanh"]) + + var_list.append( [w("action_net.bias"), w("action_net.weight"), "none"] ) # add final layer + + with open(output_file,"wb") as f: + pickle.dump(var_list, f, protocol=4) # protocol 4 is backward compatible with Python 3.4 + + + +class Cyclic_Callback(BaseCallback): + ''' Stable baselines custom callback ''' + def __init__(self, freq, function): + super(Cyclic_Callback, self).__init__(1) + self.freq = freq + self.function = function + + def _on_step(self) -> bool: + if self.n_calls % self.freq == 0: + self.function() + return True # If the callback returns False, training is aborted early + +class Export_Callback(BaseCallback): + ''' Stable baselines custom callback ''' + def __init__(self, freq, load_path, export_name): + super(Export_Callback, self).__init__(1) + self.freq = freq + self.load_path = load_path + self.export_name = export_name + + def _on_step(self) -> bool: + if self.n_calls % self.freq == 0: + path = os.path.join(self.load_path, f"model_{self.num_timesteps}_steps.zip") + Train_Base.export_model(path, f"./scripts/gyms/export/{self.export_name}") + return True # If the callback returns False, training is aborted early \ No newline at end of file diff --git a/scripts/commons/UI.py b/scripts/commons/UI.py new file mode 100644 index 0000000..e1869e4 --- /dev/null +++ b/scripts/commons/UI.py @@ -0,0 +1,302 @@ +from itertools import zip_longest +from math import inf +import math +import numpy as np +import shutil + +class UI(): + console_width = 80 + console_height = 24 + + @staticmethod + def read_particle(prompt, str_options, dtype=str, interval=[-inf,inf]): + ''' + Read particle from user from a given dtype or from a str_options list + + Parameters + ---------- + prompt : `str` + prompt to show user before reading input + str_options : `list` + list of str options (in addition to dtype if dtype is not str) + dtype : `class` + if dtype is str, then user must choose a value from str_options, otherwise it can also send a dtype value + interval : `list` + [>=min,= interval[0] and inp < interval[1]: + return inp, False + except: + pass + + print("Error: illegal input! Options:", str_options, f" or {dtype}" if dtype != str else "") + + @staticmethod + def read_int(prompt, min, max): + ''' + Read int from user in a given interval + :param prompt: prompt to show user before reading input + :param min: minimum input (inclusive) + :param max: maximum input (exclusive) + :return: choice + ''' + while True: + inp = input(prompt) + try: + inp = int(inp) + assert inp >= min and inp < max + return inp + except: + print(f"Error: illegal input! Choose number between {min} and {max-1}") + + @staticmethod + def print_table(data, titles=None, alignment=None, cols_width=None, cols_per_title=None, margins=None, numbering=None, prompt=None): + ''' + Print table + + Parameters + ---------- + data : `list` + list of columns, where each column is a list of items + titles : `list` + list of titles for each column, default is `None` (no titles) + alignment : `list` + list of alignments per column (excluding titles), default is `None` (left alignment for all cols) + cols_width : `list` + list of widths per column, default is `None` (fit to content) + Positive values indicate a fixed column width + Zero indicates that the column will fit its content + cols_per_title : `list` + maximum number of subcolumns per title, default is `None` (1 subcolumn per title) + margins : `list` + number of added leading and trailing spaces per column, default is `None` (margin=2 for all columns) + numbering : `list` + list of booleans per columns, indicating whether to assign numbers to each option + prompt : `str` + the prompt string, if given, is printed after the table before reading input + + Returns + ------- + index : `int` + returns global index of selected item (relative to table) + col_index : `int` + returns local index of selected item (relative to column) + column : `int` + returns number of column of selected item (starts at 0) + * if `numbering` or `prompt` are `None`, `None` is returned + + + Example + ------- + titles = ["Name","Age"] + data = [[John,Graciete], [30,50]] + alignment = ["<","^"] # 1st column is left-aligned, 2nd is centered + cols_width = [10,5] # 1st column's width=10, 2nd column's width=5 + margins = [3,3] + numbering = [True,False] # prints: [0-John,1-Graciete][30,50] + prompt = "Choose a person:" + ''' + + #--------------------------------------------- parameters + cols_no = len(data) + + if alignment is None: + alignment = ["<"]*cols_no + + if cols_width is None: + cols_width = [0]*cols_no + + if numbering is None: + numbering = [False]*cols_no + any_numbering = False + else: + any_numbering = True + + if margins is None: + margins = [2]*cols_no + + # Fit column to content + margin, if required + subcol = [] # subcolumn length and widths + for i in range(cols_no): + subcol.append([[],[]]) + if cols_width[i] == 0: + numbering_width = 4 if numbering[i] else 0 + if cols_per_title is None or cols_per_title[i] < 2: + cols_width[i] = max([len(str(item))+numbering_width for item in data[i]]) + margins[i]*2 + else: + subcol[i][0] = math.ceil(len(data[i])/cols_per_title[i]) # subcolumn maximum length + cols_per_title[i] = math.ceil(len(data[i])/subcol[i][0]) # reduce number of columns as needed + cols_width[i] = margins[i]*(1+cols_per_title[i]) - (1 if numbering[i] else 0) # remove one if numbering, same as when printing + for j in range(cols_per_title[i]): + subcol_data_width = max([len(str(item))+numbering_width for item in data[i][j*subcol[i][0]:j*subcol[i][0]+subcol[i][0]]]) + cols_width[i] += subcol_data_width # add subcolumn data width to column width + subcol[i][1].append(subcol_data_width) # save subcolumn data width + + if titles is not None: # expand to acomodate titles if needed + cols_width[i] = max(cols_width[i], len(titles[i]) + margins[i]*2 ) + + if any_numbering: + no_of_items=0 + cumulative_item_per_col=[0] # useful for getting the local index + for i in range(cols_no): + assert type(data[i]) == list, "In function 'print_table', 'data' must be a list of lists!" + + if numbering[i]: + data[i] = [f"{n+no_of_items:3}-{d}" for n,d in enumerate(data[i])] + no_of_items+=len(data[i]) + cumulative_item_per_col.append(no_of_items) + + table_width = sum(cols_width)+cols_no-1 + + #--------------------------------------------- col titles + print(f'{"="*table_width}') + if titles is not None: + for i in range(cols_no): + print(f'{titles[i]:^{cols_width[i]}}', end='|' if i < cols_no - 1 else '') + print() + for i in range(cols_no): + print(f'{"-"*cols_width[i]}', end='+' if i < cols_no - 1 else '') + print() + + #--------------------------------------------- merge subcolumns + if cols_per_title is not None: + for i,col in enumerate(data): + if cols_per_title[i] < 2: + continue + for k in range(subcol[i][0]): # create merged items + col[k] = (" "*margins[i]).join( f'{col[item]:{alignment[i]}{subcol[i][1][subcol_idx]}}' + for subcol_idx, item in enumerate(range(k,len(col),subcol[i][0])) ) + del col[subcol[i][0]:] # delete repeated items + + #--------------------------------------------- col items + for line in zip_longest(*data): + for i,item in enumerate(line): + l_margin = margins[i]-1 if numbering[i] else margins[i] # adjust margins when there are numbered options + item = "" if item is None else f'{" "*l_margin}{item}{" "*margins[i]}' # add margins + print(f'{item:{alignment[i]}{cols_width[i]}}', end='') + if i < cols_no - 1: + print(end='|') + print(end="\n") + print(f'{"="*table_width}') + + #--------------------------------------------- prompt + if prompt is None: + return None + + if not any_numbering: + print(prompt) + return None + + index = UI.read_int(prompt, 0, no_of_items) + + for i,n in enumerate(cumulative_item_per_col): + if index < n: + return index, index-cumulative_item_per_col[i-1], i-1 + + raise ValueError('Failed to catch illegal input') + + + @staticmethod + def print_list(data, numbering=True, prompt=None, divider=" | ", alignment="<", min_per_col=6): + ''' + Print list - prints list, using as many columns as possible + + Parameters + ---------- + data : `list` + list of items + numbering : `bool` + assigns number to each option + prompt : `str` + the prompt string, if given, is printed after the table before reading input + divider : `str` + string that divides columns + alignment : `str` + f-string style alignment ( '<', '>', '^' ) + min_per_col : int + avoid splitting columns with fewer items + + Returns + ------- + item : `int`, item + returns tuple with global index of selected item and the item object, + or `None` (if `numbering` or `prompt` are `None`) + + ''' + + WIDTH = shutil.get_terminal_size()[0] + + data_size = len(data) + items = [] + items_len = [] + + #--------------------------------------------- Add numbers, margins and divider + for i in range(data_size): + number = f"{i}-" if numbering else "" + items.append( f"{divider}{number}{data[i]}" ) + items_len.append( len(items[-1]) ) + + max_cols = np.clip((WIDTH+len(divider)) // min(items_len),1,math.ceil(data_size/max(min_per_col,1))) # width + len(divider) because it is not needed in last col + + #--------------------------------------------- Check maximum number of columns, considering content width (min:1) + for i in range(max_cols,0,-1): + cols_width = [] + cols_items = [] + table_width = 0 + a,b = divmod(data_size,i) + for col in range(i): + start = a*col + min(b,col) + end = start+a+(1 if col (px,py,pz,fx,fy,fz)* + self.obs[18:24] = r.frp.get('rf', (0,0,0,0,0,0)) # right foot: relative point of origin (p) and force vector (f) -> (px,py,pz,fx,fy,fz)* + self.obs[15:18] /= 100 # naive normalization of force vector + self.obs[21:24] /= 100 # naive normalization of force vector + self.obs[24:44] = r.joints_position[2:22] /100 # position of all joints except head & toes (for robot type 4) + self.obs[44:64] = r.joints_speed[2:22] /6.1395 # speed of all joints except head & toes (for robot type 4) + # *if foot is not touching the ground, then (px=0,py=0,pz=0,fx=0,fy=0,fz=0) + + if init: # the walking parameters refer to the last parameters in effect (after a reset, they are pointless) + self.obs[64] = self.step_default_dur /10 # step duration in time steps + self.obs[65] = self.step_default_z_span *20 # vertical movement span + self.obs[66] = self.step_default_z_max # relative extension of support leg + self.obs[67] = 1 # step progress + self.obs[68] = 1 # 1 if left leg is active + self.obs[69] = 0 # 1 if right leg is active + else: + self.obs[64] = self.step_obj.step_generator.ts_per_step /10 # step duration in time steps + self.obs[65] = self.step_obj.step_generator.swing_height *20 # vertical movement span + self.obs[66] = self.step_obj.step_generator.max_leg_extension / self.step_obj.leg_length # relative extension of support leg + self.obs[67] = self.step_obj.step_generator.external_progress # step progress + self.obs[68] = float(self.step_obj.step_generator.state_is_left_active) # 1 if left leg is active + self.obs[69] = float(not self.step_obj.step_generator.state_is_left_active) # 1 if right leg is active + + ''' + Expected observations for walking parameters/state (example): + Time step R 0 1 2 0 1 2 3 4 + Progress 1 0 .5 1 0 .25 .5 .75 1 + Left leg active T F F F T T T T T + Parameters A A A B B B B B C + Example note: (A) has a step duration of 3ts, (B) has a step duration of 5ts + ''' + + return self.obs + + def sync(self): + ''' Run a single simulation step ''' + r = self.player.world.robot + self.player.scom.commit_and_send( r.get_command() ) + self.player.scom.receive() + + + def reset(self): + ''' + Reset and stabilize the robot + Note: for some behaviors it would be better to reduce stabilization or add noise + ''' + + self.step_counter = 0 + r = self.player.world.robot + + for _ in range(25): + self.player.scom.unofficial_beam((-14,0,0.50),0) # beam player continuously (floating above ground) + self.player.behavior.execute("Zero_Bent_Knees") + self.sync() + + # beam player to ground + self.player.scom.unofficial_beam((-14,0,r.beam_height),0) + r.joints_target_speed[0] = 0.01 # move head to trigger physics update (rcssserver3d bug when no joint is moving) + self.sync() + + # stabilize on ground + for _ in range(7): + self.player.behavior.execute("Zero_Bent_Knees") + self.sync() + + # memory variables + self.lastx = r.cheat_abs_pos[0] + self.act = np.zeros(self.no_of_actions,np.float32) + + return self.observe(True) + + def render(self, mode='human', close=False): + return + + def close(self): + Draw.clear_all() + self.player.terminate() + + def step(self, action): + + r = self.player.world.robot + + # exponential moving average + self.act = 0.4 * self.act + 0.6 * action + + # execute Step behavior to extract the target positions of each leg (we will override these targets) + if self.step_counter == 0: + ''' + The first time step will change the parameters of the next footstep + It uses default parameters so that the agent can anticipate the next generated pose + Reason: the agent decides the parameters during the previous footstep + ''' + self.player.behavior.execute("Step", self.step_default_dur, self.step_default_z_span, self.step_default_z_max) + else: + + step_zsp = np.clip(self.step_default_z_span + self.act[20]/300, 0, 0.07) + step_zmx = np.clip(self.step_default_z_max + self.act[21]/30, 0.6, 0.9) + + self.player.behavior.execute("Step", self.step_default_dur, step_zsp, step_zmx) + + + # add action as residuals to Step behavior (the index of these actions is not the typical index because both head joints are excluded) + new_action = self.act[:20] * 2 # scale up actions to motivate exploration + new_action[[0,2,4,6,8,10]] += self.step_obj.values_l + new_action[[1,3,5,7,9,11]] += self.step_obj.values_r + new_action[12] -= 90 # arms down + new_action[13] -= 90 # arms down + new_action[16] += 90 # untwist arms + new_action[17] += 90 # untwist arms + new_action[18] += 90 # elbows at 90 deg + new_action[19] += 90 # elbows at 90 deg + + r.set_joints_target_position_direct( # commit actions: + slice(2,22), # act on all joints except head & toes (for robot type 4) + new_action, # target joint positions + harmonize=False # there is no point in harmonizing actions if the targets change at every step + ) + + self.sync() # run simulation step + self.step_counter += 1 + + reward = r.cheat_abs_pos[0] - self.lastx + self.lastx = r.cheat_abs_pos[0] + + # terminal state: the robot is falling or timeout + terminal = r.cheat_abs_pos[2] < 0.3 or self.step_counter > 300 + + return self.observe(), reward, terminal, {} + + + + + +class Train(Train_Base): + def __init__(self, script) -> None: + super().__init__(script) + + + def train(self, args): + + #--------------------------------------- Learning parameters + n_envs = min(16, os.cpu_count()) + n_steps_per_env = 1024 # RolloutBuffer is of size (n_steps_per_env * n_envs) + minibatch_size = 64 # should be a factor of (n_steps_per_env * n_envs) + total_steps = 30000000 + learning_rate = 3e-4 + folder_name = f'Basic_Run_R{self.robot_type}' + model_path = f'./scripts/gyms/logs/{folder_name}/' + + print("Model path:", model_path) + + #--------------------------------------- Run algorithm + def init_env(i_env): + def thunk(): + return Basic_Run( self.ip , self.server_p + i_env, self.monitor_p_1000 + i_env, self.robot_type, False ) + return thunk + + servers = Server( self.server_p, self.monitor_p_1000, n_envs+1 ) #include 1 extra server for testing + + env = SubprocVecEnv( [init_env(i) for i in range(n_envs)] ) + eval_env = SubprocVecEnv( [init_env(n_envs)] ) + + try: + if "model_file" in args: # retrain + model = PPO.load( args["model_file"], env=env, device="cpu", n_envs=n_envs, n_steps=n_steps_per_env, batch_size=minibatch_size, learning_rate=learning_rate ) + else: # train new model + model = PPO( "MlpPolicy", env=env, verbose=1, n_steps=n_steps_per_env, batch_size=minibatch_size, learning_rate=learning_rate, device="cpu" ) + + model_path = self.learn_model( model, total_steps, model_path, eval_env=eval_env, eval_freq=n_steps_per_env*20, save_freq=n_steps_per_env*200, backup_env_file=__file__ ) + except KeyboardInterrupt: + sleep(1) # wait for child processes + print("\nctrl+c pressed, aborting...\n") + servers.kill() + return + + env.close() + eval_env.close() + servers.kill() + + + def test(self, args): + + # Uses different server and monitor ports + server = Server( self.server_p-1, self.monitor_p, 1 ) + env = Basic_Run( self.ip, self.server_p-1, self.monitor_p, self.robot_type, True ) + model = PPO.load( args["model_file"], env=env ) + + try: + self.export_model( args["model_file"], args["model_file"]+".pkl", False ) # Export to pkl to create custom behavior + self.test_model( model, env, log_path=args["folder_dir"], model_path=args["folder_dir"] ) + except KeyboardInterrupt: + print() + + env.close() + server.kill() + + +''' +The learning process takes several hours. +A video with the results can be seen at: +https://imgur.com/a/dC2V6Et + +Stats: +- Avg. reward: 7.7 +- Avg. ep. length: 5.5s (episode is limited to 6s) +- Max. reward: 9.3 (speed: 1.55m/s) + +State space: +- Composed of all joint positions + torso height +- Stage of the underlying Step behavior + +Reward: +- Displacement in the x-axis (it can be negative) +- Note that cheat and visual data is only updated every 3 steps +''' diff --git a/scripts/gyms/Fall.py b/scripts/gyms/Fall.py new file mode 100644 index 0000000..9f48ac8 --- /dev/null +++ b/scripts/gyms/Fall.py @@ -0,0 +1,214 @@ +from agent.Base_Agent import Base_Agent as Agent +from world.commons.Draw import Draw +from stable_baselines3 import PPO +from stable_baselines3.common.vec_env import SubprocVecEnv +from scripts.commons.Server import Server +from scripts.commons.Train_Base import Train_Base +from time import sleep +import os, gym +import numpy as np + +''' +Objective: +Learn how to fall (simplest example) +---------- +- class Fall: implements an OpenAI custom gym +- class Train: implements algorithms to train a new model or test an existing model +''' + +class Fall(gym.Env): + def __init__(self, ip, server_p, monitor_p, r_type, enable_draw) -> None: + + self.robot_type = r_type + + # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name, Enable Log, Enable Draw + self.player = Agent(ip, server_p, monitor_p, 1, self.robot_type, "Gym", True, enable_draw) + self.step_counter = 0 # to limit episode size + + # State space + self.no_of_joints = self.player.world.robot.no_of_joints + self.obs = np.zeros(self.no_of_joints + 1, np.float32) # joints + torso height + self.observation_space = gym.spaces.Box(low=np.full(len(self.obs),-np.inf,np.float32), high=np.full(len(self.obs),np.inf,np.float32), dtype=np.float32) + + # Action space + MAX = np.finfo(np.float32).max + no_of_actions = self.no_of_joints + self.action_space = gym.spaces.Box(low=np.full(no_of_actions,-MAX,np.float32), high=np.full(no_of_actions,MAX,np.float32), dtype=np.float32) + + # Check if cheats are enabled + assert np.any(self.player.world.robot.cheat_abs_pos), "Cheats are not enabled! Run_Utils.py -> Server -> Cheats" + + + def observe(self): + + r = self.player.world.robot + + for i in range(self.no_of_joints): + self.obs[i] = r.joints_position[i] / 100 # naive scale normalization + + self.obs[self.no_of_joints] = r.cheat_abs_pos[2] # head.z (alternative: r.loc_head_z) + + return self.obs + + def sync(self): + ''' Run a single simulation step ''' + r = self.player.world.robot + self.player.scom.commit_and_send( r.get_command() ) + self.player.scom.receive() + + + def reset(self): + ''' + Reset and stabilize the robot + Note: for some behaviors it would be better to reduce stabilization or add noise + ''' + + self.step_counter = 0 + r = self.player.world.robot + + for _ in range(25): + self.player.scom.unofficial_beam((-3,0,0.50),0) # beam player continuously (floating above ground) + self.player.behavior.execute("Zero") + self.sync() + + # beam player to ground + self.player.scom.unofficial_beam((-3,0,r.beam_height),0) + r.joints_target_speed[0] = 0.01 # move head to trigger physics update (rcssserver3d bug when no joint is moving) + self.sync() + + # stabilize on ground + for _ in range(7): + self.player.behavior.execute("Zero") + self.sync() + + return self.observe() + + def render(self, mode='human', close=False): + return + + def close(self): + Draw.clear_all() + self.player.terminate() + + def step(self, action): + + r = self.player.world.robot + r.set_joints_target_position_direct( # commit actions: + slice(self.no_of_joints), # act on all available joints + action*10, # scale actions up to motivate early exploration + harmonize=False # there is no point in harmonizing actions if the targets change at every step + ) + + self.sync() # run simulation step + self.step_counter += 1 + self.observe() + + if self.obs[-1] < 0.15: # terminal state: the robot has fallen successfully + return self.obs, 1, True, {} # Reward: 1 (this reward will motivate a fast reaction if the return is discounted) + elif self.step_counter > 150: # terminal state: 3s passed and robot has not fallen (may be stuck) + return self.obs, 0, True, {} + else: + return self.obs, 0, False, {} # Reward: 0 + + + + + +class Train(Train_Base): + def __init__(self, script) -> None: + super().__init__(script) + + + def train(self, args): + + #--------------------------------------- Learning parameters + n_envs = min(4, os.cpu_count()) + n_steps_per_env = 128 # RolloutBuffer is of size (n_steps_per_env * n_envs) (*RV: >=2048) + minibatch_size = 64 # should be a factor of (n_steps_per_env * n_envs) + total_steps = 50000 # (*RV: >=10M) + learning_rate = 30e-4 # (*RV: 3e-4) + # *RV -> Recommended value for more complex environments + folder_name = f'Fall_R{self.robot_type}' + model_path = f'./scripts/gyms/logs/{folder_name}/' + + print("Model path:", model_path) + + #--------------------------------------- Run algorithm + def init_env(i_env): + def thunk(): + return Fall( self.ip , self.server_p + i_env, self.monitor_p_1000 + i_env, self.robot_type, False ) + return thunk + + servers = Server( self.server_p, self.monitor_p_1000, n_envs+1 ) #include 1 extra server for testing + + env = SubprocVecEnv( [init_env(i) for i in range(n_envs)] ) + eval_env = SubprocVecEnv( [init_env(n_envs)] ) + + try: + if "model_file" in args: # retrain + model = PPO.load( args["model_file"], env=env, n_envs=n_envs, n_steps=n_steps_per_env, batch_size=minibatch_size, learning_rate=learning_rate ) + else: # train new model + model = PPO( "MlpPolicy", env=env, verbose=1, n_steps=n_steps_per_env, batch_size=minibatch_size, learning_rate=learning_rate ) + + model_path = self.learn_model( model, total_steps, model_path, eval_env=eval_env, eval_freq=n_steps_per_env*10, save_freq=n_steps_per_env*20, backup_env_file=__file__ ) + except KeyboardInterrupt: + sleep(1) # wait for child processes + print("\nctrl+c pressed, aborting...\n") + servers.kill() + return + + env.close() + eval_env.close() + servers.kill() + + + def test(self, args): + + # Uses different server and monitor ports + server = Server( self.server_p-1, self.monitor_p, 1 ) + env = Fall( self.ip, self.server_p-1, self.monitor_p, self.robot_type, True ) + model = PPO.load( args["model_file"], env=env ) + + try: + self.export_model( args["model_file"], args["model_file"]+".pkl", False ) # Export to pkl to create custom behavior + self.test_model( model, env, log_path=args["folder_dir"], model_path=args["folder_dir"] ) + except KeyboardInterrupt: + print() + + env.close() + server.kill() + + +''' +The learning process takes about 5 minutes. +A video with the results can be seen at: +https://imgur.com/a/KvpXS41 + +State space: +- Composed of all joint positions + torso height +- The number of joint positions is different for robot type 4, so the models are not interchangeable +- For this example, this problem can be avoided by using only the first 22 joints and actuators + +Reward: +- The reward for falling is 1, which means that after a while every episode will have a r=1. +- What is the incetive for the robot to fall faster? Discounted return. + In every state, the algorithm will seek short-term rewards. +- During training, the best model is saved according to the average return, which is almost always 1. + Therefore, the last model will typically be superior for this example. + +Expected evolution of episode length: + 3s|o + |o + | o + | o + | oo + | ooooo + 0.4s| oooooooooooooooo + |------------------------------> time + + +This example scales poorly with the number of CPUs because: +- It uses a small rollout buffer (n_steps_per_env * n_envs) +- The simulation workload is light +- For these reasons, the IPC overhead is significant +''' diff --git a/scripts/utils/Beam.py b/scripts/utils/Beam.py new file mode 100644 index 0000000..5d71647 --- /dev/null +++ b/scripts/utils/Beam.py @@ -0,0 +1,60 @@ + +from agent.Base_Agent import Base_Agent as Agent +from scripts.commons.Script import Script +from time import sleep + + +class Beam(): + def __init__(self, script:Script) -> None: + self.script = script + + def ask_for_input(self,prompt, default): + try: + inp=input(prompt) + return float(inp) + except ValueError: + if inp != '': + print("Illegal input:", inp, "\n") + return default + + def beam_and_update(self,x,y,rot): + r = self.player.world.robot + d = self.player.world.draw + + d.annotation((x,y,0.7), f"x:{x} y:{y} r:{rot}", d.Color.yellow, "pos_label") + + self.player.scom.unofficial_beam((x,y,r.beam_height),rot) + for _ in range(10): # run multiple times to beam and then simulate eventual collisions (e.g. goal posts) + sleep(0.03) + self.player.behavior.execute("Zero") + self.player.scom.commit_and_send( r.get_command() ) + self.player.scom.receive() + + def execute(self): + + a = self.script.args + self.player = Agent(a.i, a.p, a.m, a.u, a.r, a.t) # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name + d = self.player.world.draw + + self.player.scom.unofficial_set_play_mode("PlayOn") + + # Draw grid + for x in range(-15,16): + for y in range(-10,11): + d.point((x,y), 6, d.Color.red, "grid", False) + d.flush("grid") + + for _ in range(10): # Initialize + self.player.scom.send() + self.player.scom.receive() + + print("\nBeam player to coordinates + orientation:") + + x=y=a=0 + while True: # Beam self.player to given position + x = self.ask_for_input(f"\nInput x coordinate ('' to send {x:5} again, ctrl+c to return): ",x) + self.beam_and_update(x,y,a) + y = self.ask_for_input( f"Input y coordinate ('' to send {y:5} again, ctrl+c to return): ",y) + self.beam_and_update(x,y,a) + a = self.ask_for_input( f"Orientation -180 to 180 ('' to send {a:5} again, ctrl+c to return): ",a) + self.beam_and_update(x,y,a) \ No newline at end of file diff --git a/scripts/utils/Behaviors.py b/scripts/utils/Behaviors.py new file mode 100644 index 0000000..653c923 --- /dev/null +++ b/scripts/utils/Behaviors.py @@ -0,0 +1,55 @@ +from agent.Base_Agent import Base_Agent as Agent +from scripts.commons.Script import Script +from scripts.commons.UI import UI + +class Behaviors(): + + def __init__(self,script:Script) -> None: + self.script = script + self.player : Agent = None + + def ask_for_behavior(self): + names, descriptions = self.player.behavior.get_all_behaviors() + + UI.print_table( [names,descriptions], ["Behavior Name","Description"], numbering=[True,False]) + choice, is_str_opt = UI.read_particle('Choose behavior ("" to skip 2 time steps, "b" to beam, ctrl+c to return): ',["","b"],int,[0,len(names)]) + if is_str_opt: return choice #skip 2 time steps or quit + return names[choice] + + def sync(self): + self.player.scom.commit_and_send( self.player.world.robot.get_command() ) + self.player.scom.receive() + + def beam(self): + self.player.scom.unofficial_beam((-2.5,0,self.player.world.robot.beam_height),0) + for _ in range(5): + self.sync() + + def execute(self): + + a = self.script.args + self.player = Agent(a.i, a.p, a.m, a.u, a.r, a.t) # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name + behavior = self.player.behavior + + self.beam() + self.player.scom.unofficial_set_play_mode("PlayOn") + + # Special behaviors + special_behaviors = {"Step":(), "Basic_Kick":(0,), "Walk":((0.5,0),False,0,False,None), "Dribble":(None,None)} + + while True: + behavior_name = self.ask_for_behavior() + if behavior_name == 0: # skip 2 time steps (user request) + self.sync() + self.sync() + elif behavior_name == 1: # beam + self.beam() + else: + if behavior_name in special_behaviors: # not using execute_to_completion to abort behavior after a timeout + duration = UI.read_int("For how many time steps [1,1000]? ", 1, 1001) + for _ in range(duration): + if behavior.execute(behavior_name, *special_behaviors[behavior_name]): + break # break if behavior ends + self.sync() + else: + behavior.execute_to_completion(behavior_name) \ No newline at end of file diff --git a/scripts/utils/Drawings.py b/scripts/utils/Drawings.py new file mode 100644 index 0000000..2ffddd4 --- /dev/null +++ b/scripts/utils/Drawings.py @@ -0,0 +1,35 @@ +from time import sleep +from world.commons.Draw import Draw + + +class Drawings(): + + def __init__(self,script) -> None: + self.script = script + + def execute(self): + + # Creating a draw object is done automatically for each agent + # This is a shortcut to draw shapes without creating an agent + # Usually, we can access the object through player.world.draw + a = self.script.args + draw = Draw(True, 0, a.i, 32769) + + print("\nPress ctrl+c to return.") + + while True: + for i in range(100): + sleep(0.02) + + draw.circle( (0,0),i/10,2, Draw.Color.green_light,"green") + draw.circle( (0,0),i/9,2, Draw.Color.red,"red") + draw.sphere( (0,0,5-i/20),0.2, Draw.Color.red,"ball" ) + draw.annotation((0,0,1), "Hello!", Draw.Color.cyan, "text") + draw.arrow( (0,0,5), (0,0,5-i/25), 0.5, 4, Draw.Color.get(127,50,255), "my_arrow") + + #draw pyramid + draw.polygon(((2,0,0),(3,0,0),(3,1,0),(2,1,0)), Draw.Color.blue, 255, "solid", False) + draw.line( (2,0,0), (2.5,0.5,1), 2, Draw.Color.cyan, "solid", False) + draw.line( (3,0,0), (2.5,0.5,1), 2, Draw.Color.cyan, "solid", False) + draw.line( (2,1,0), (2.5,0.5,1), 2, Draw.Color.cyan, "solid", False) + draw.line( (3,1,0), (2.5,0.5,1), 2, Draw.Color.cyan, "solid", True) \ No newline at end of file diff --git a/scripts/utils/Dribble.py b/scripts/utils/Dribble.py new file mode 100644 index 0000000..fb776e8 --- /dev/null +++ b/scripts/utils/Dribble.py @@ -0,0 +1,54 @@ +from agent.Agent import Agent +from agent.Base_Agent import Base_Agent +from scripts.commons.Script import Script +import numpy as np + +''' +Objective: +---------- +Dribble and score +''' + +class Dribble(): + def __init__(self, script:Script) -> None: + self.script = script + + def execute(self): + + a = self.script.args + + # Args: Server IP, Agent Port, Monitor Port, Uniform No., [Robot Type] (for Base_Agent), Team name, Enable Log, Enable Draw + self.script.batch_create(Base_Agent, ((a.i,a.p,a.m,a.u,a.r,a.t,True,True),)) # one dribbler + self.script.batch_create(Agent, ((a.i,a.p,a.m,u,"Opponent",False,False) for u in range(1,2))) # 1 opponent (normal agent) + + p : Base_Agent = self.script.players[0] + p.path_manager.draw_options(enable_obstacles=True, enable_path=True) + + behavior = p.behavior + w = p.world + r = w.robot + d = w.draw + + p.scom.unofficial_beam((-3,0,r.beam_height),0) + p.scom.unofficial_set_play_mode("PlayOn") + print("\nPress ctrl+c to return.") + + while True: + + if w.play_mode == w.M_THEIR_KICKOFF: + p.scom.unofficial_set_play_mode("PlayOn") + + # execute dribbler + if behavior.is_ready("Get_Up") or w.play_mode_group in [w.MG_ACTIVE_BEAM, w.MG_PASSIVE_BEAM]: + p.scom.unofficial_beam((*(w.ball_abs_pos[:2]-(1,0)),r.beam_height),0) + behavior.execute("Zero_Bent_Knees") + else: + behavior.execute("Dribble",None,None) + d.annotation(r.loc_head_position+(0,0,0.2),f"{np.linalg.norm(r.get_head_abs_vel(40)[:2]):.2f}",d.Color.white,"vel_annotation") + p.scom.commit_and_send( r.get_command() ) + + # execute opponents as normal agents + self.script.batch_execute_agent(slice(1,None)) + + # all players wait for server to send feedback + self.script.batch_receive() \ No newline at end of file diff --git a/scripts/utils/Fwd_Kinematics.py b/scripts/utils/Fwd_Kinematics.py new file mode 100644 index 0000000..af6f6a8 --- /dev/null +++ b/scripts/utils/Fwd_Kinematics.py @@ -0,0 +1,115 @@ +from agent.Base_Agent import Base_Agent as Agent +from scripts.commons.Script import Script +from world.commons.Draw import Draw +import numpy as np + +class Fwd_Kinematics(): + + def __init__(self,script:Script) -> None: + self.script = script + self.cycle_duration = 200 #steps + + def draw_cycle(self): + + #Draw position of body parts + for _ in range(self.cycle_duration): + self.script.batch_execute_behavior("Squat") + self.script.batch_commit_and_send() + self.script.batch_receive() + + p : Agent + for p in self.script.players: + if p.world.vision_is_up_to_date and not p.world.robot.loc_is_up_to_date: + p.world.draw.annotation(p.world.robot.cheat_abs_pos, "Not enough visual data! Using IMU", Draw.Color.red,"localization") + + for key, val in p.world.robot.body_parts.items(): + rp = val.transform.get_translation() + pos = p.world.robot.loc_head_to_field_transform(rp,False) + label_rp = np.array([rp[0]-0.0001,rp[1]*0.5,0]) + label_rp /= np.linalg.norm(label_rp) / 0.4 #labels at 0.4m from body part + label = p.world.robot.loc_head_to_field_transform(rp+label_rp,False) + p.world.draw.line(pos,label,2,Draw.Color.green_light,key,False) + p.world.draw.annotation(label,key,Draw.Color.red,key) + + rp = p.world.robot.body_parts['lfoot'].transform((0.08,0,0)) + ap = p.world.robot.loc_head_to_field_transform(rp,False) + p.world.draw.line(ap,ap+(0,0,0.1),1,Draw.Color.red,"soup",False) + rp = p.world.robot.body_parts['lfoot'].transform((-0.08,0,0)) + ap = p.world.robot.loc_head_to_field_transform(rp,False) + p.world.draw.line(ap,ap+(0,0,0.1),1,Draw.Color.red,"soup",False) + rp = p.world.robot.body_parts['lfoot'].transform((0,0.04,0)) + ap = p.world.robot.loc_head_to_field_transform(rp,False) + p.world.draw.line(ap,ap+(0,0,0.1),1,Draw.Color.red,"soup",False) + rp = p.world.robot.body_parts['lfoot'].transform((0,-0.04,0)) + ap = p.world.robot.loc_head_to_field_transform(rp,False) + p.world.draw.line(ap,ap+(0,0,0.1),1,Draw.Color.red,"soup",True) + + Draw.clear_all() + + #Draw position of joints + for _ in range(self.cycle_duration): + self.script.batch_execute_behavior("Squat") + self.script.batch_commit_and_send() + self.script.batch_receive() + + for p in self.script.players: + if p.world.vision_is_up_to_date and not p.world.robot.loc_is_up_to_date: + p.world.draw.annotation(p.world.robot.cheat_abs_pos, "Not enough visual data! Using IMU", Draw.Color.red,"localization") + + zstep = 0.05 + label_z = [0,0,0,0,zstep,zstep,2*zstep,2*zstep,0,0,0,0,zstep,zstep,0,0,zstep,zstep,2*zstep,2*zstep,3*zstep,3*zstep,0,0] + for j, transf in enumerate(p.world.robot.joints_transform): + rp = transf.get_translation() + pos = p.world.robot.loc_head_to_field_transform(rp,False) + j_name = str(j) + label_rp = np.array([rp[0]-0.0001,rp[1]*0.5,0]) + label_rp /= np.linalg.norm(label_rp) / 0.4 #labels at 0.4m from body part + label_rp += (0,0,label_z[j]) + label = p.world.robot.loc_head_to_field_transform(rp+label_rp,False) + p.world.draw.line( pos,label,2,Draw.Color.green_light,j_name,False) + p.world.draw.annotation( label,j_name,Draw.Color.cyan,j_name) + + + Draw.clear_all() + + #Draw orientation of body parts + for _ in range(self.cycle_duration): + self.script.batch_execute_behavior("Squat") + self.script.batch_commit_and_send() + self.script.batch_receive() + + p : Agent + for p in self.script.players: + if p.world.vision_is_up_to_date and not p.world.robot.loc_is_up_to_date: + p.world.draw.annotation(p.world.robot.cheat_abs_pos, "Not enough visual data! Using IMU", Draw.Color.red,"localization") + + for key in p.world.robot.body_parts: + #Select only some body parts + if key not in ['head', 'torso', 'llowerarm', 'rlowerarm', 'lthigh', 'rthigh', 'lshank', 'rshank', 'lfoot', 'rfoot']: continue + bpart_abs_pos = p.world.robot.get_body_part_to_field_transform(key).translate((0.1,0,0)) #10cm in front of body part + x_axis = bpart_abs_pos((0.05,0,0),False) + y_axis = bpart_abs_pos((0,0.05,0),False) + z_axis = bpart_abs_pos((0,0,0.05),False) + axes_0 = bpart_abs_pos.get_translation() + p.world.draw.line( axes_0,x_axis,2,Draw.Color.green_light,key,False) + p.world.draw.line( axes_0,y_axis,2,Draw.Color.blue,key,False) + p.world.draw.line( axes_0,z_axis,2,Draw.Color.red,key) + + Draw.clear_all() + + + + def execute(self): + + a = self.script.args + + # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name, Enable Log, Enable Draw + self.script.batch_create(Agent, ((a.i,a.p,a.m,u,u-1,a.t,True,True) for u in range(1,6)) ) + + #Beam players + self.script.batch_unofficial_beam( [(-2,i*4-10,0.5,i*45) for i in range(5)] ) + + print("\nPress ctrl+c to return.") + + while True: + self.draw_cycle() \ No newline at end of file diff --git a/scripts/utils/Get_Up.py b/scripts/utils/Get_Up.py new file mode 100644 index 0000000..f95464c --- /dev/null +++ b/scripts/utils/Get_Up.py @@ -0,0 +1,45 @@ +from agent.Base_Agent import Base_Agent as Agent +from itertools import count +from scripts.commons.Script import Script +import numpy as np + +''' +Objective: +---------- +Fall and get up +''' + +class Get_Up(): + def __init__(self, script:Script) -> None: + self.script = script + self.player : Agent = None + + def sync(self): + r = self.player.world.robot + self.player.scom.commit_and_send( r.get_command() ) + self.player.scom.receive() + + def execute(self): + + a = self.script.args + player = self.player = Agent(a.i, a.p, a.m, a.u, a.r, a.t) # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name + behavior = player.behavior + r = player.world.robot + + player.scom.commit_beam((-3,0),0) + print("\nPress ctrl+c to return.") + + for i in count(): + rnd = np.random.uniform(-6,6,r.no_of_joints) + + # Fall + while r.loc_head_z > 0.3 and r.imu_torso_inclination < 50: + if i < 4: + behavior.execute(["Fall_Front","Fall_Back","Fall_Left","Fall_Right"][i % 4]) # First, fall deterministically + else: + r.joints_target_speed[:] = rnd # Second, fall randomly + self.sync() + + # Get up + behavior.execute_to_completion("Get_Up") + behavior.execute_to_completion("Zero_Bent_Knees") diff --git a/scripts/utils/IMU.py b/scripts/utils/IMU.py new file mode 100644 index 0000000..8fedfd1 --- /dev/null +++ b/scripts/utils/IMU.py @@ -0,0 +1,179 @@ +from agent.Base_Agent import Base_Agent as Agent +from math_ops.Matrix_3x3 import Matrix_3x3 +from math_ops.Matrix_4x4 import Matrix_4x4 +from scripts.commons.Script import Script +from world.commons.Draw import Draw +from world.Robot import Robot +import numpy as np + +''' +Objective: +---------- +Demonstrate the accuracy of the IMU +Robot.imu_(...) variables are based on the visual localizer algorithm and the IMU when no visual data is available. +If visual data is not available for longer than 0.2 seconds, the robot's position is frozen and the velocity decays to zero. +The rotation computed by the IMU is so accurate that it is never frozen, no matter how long the robot goes without visual data. +It is almost always safe to use IMU data for rotation. +Known issues: the accelerometer is not reliable in the presence of "instant" acceleration peaks, due to its low sample rate (50Hz) + this limitation impacts the translation estimation during crashes (e.g. falling, crashing against other players) +''' + +class IMU(): + + def __init__(self,script:Script) -> None: + self.script = script + self.player : Agent = None + self.cycle = 0 + + self.imu_torso_to_field_rotation = [Matrix_3x3() for _ in range(3)] + self.imu_torso_to_field_transform = [Matrix_4x4() for _ in range(3)] + self.imu_head_to_field_transform = [Matrix_4x4() for _ in range(3)] + self.imu_torso_position = np.zeros((3,3)) + self.imu_torso_velocity = np.zeros((3,3)) + self.imu_torso_acceleration = np.zeros((3,3)) + self.imu_torso_next_position = np.zeros((3,3)) + self.imu_torso_next_velocity = np.zeros((3,3)) + self.imu_CoM_position = np.zeros((3,3)) + self.colors = [Draw.Color.green_light, Draw.Color.yellow, Draw.Color.red] + + def act(self): + r = self.player.world.robot + joint_indices = [r.J_LLEG_PITCH, + r.J_LKNEE, + r.J_LFOOT_PITCH, + r.J_LARM_ROLL, + r.J_RLEG_PITCH, + r.J_RKNEE, + r.J_RFOOT_PITCH, + r.J_RARM_ROLL] + + amplitude = [1,0.93,1,1,1][r.type] + + self.cycle += 1 + if self.cycle < 50: + r.set_joints_target_position_direct(joint_indices, np.array([32+10,-64,32, 45, 40+10,-80,40, 0])*amplitude) + elif self.cycle < 100: + r.set_joints_target_position_direct(joint_indices, np.array([ -10, 0, 0, 0, -10, 0, 0, 0])*amplitude) + elif self.cycle < 150: + r.set_joints_target_position_direct(joint_indices, np.array([40+10,-80,40, 0, 32+10,-64,32, 45])*amplitude) + elif self.cycle < 200: + r.set_joints_target_position_direct(joint_indices, np.array([ -10, 0, 0, 0, -10, 0, 0, 0])*amplitude) + else: + self.cycle = 0 + + self.player.scom.commit_and_send( r.get_command() ) + self.player.scom.receive() + + def act2(self): + r = self.player.world.robot + self.player.behavior.execute("Walk", (0.2,0), False, 5, False, None ) # Args: target, is_target_abs, ori, is_ori_abs, distance + self.player.scom.commit_and_send( r.get_command() ) + self.player.scom.receive() + + def draw_player_reference_frame(self,i): + pos = self.imu_torso_position[i] + xvec = self.imu_torso_to_field_rotation[i].multiply((1,0,0)) + pos + yvec = self.imu_torso_to_field_rotation[i].multiply((0,1,0)) + pos + zvec = self.imu_torso_to_field_rotation[i].multiply((0,0,1)) + pos + self.player.world.draw.arrow(pos, xvec, 0.2, 2, self.colors[i], "IMU"+str(i), False) + self.player.world.draw.arrow(pos, yvec, 0.2, 2, self.colors[i], "IMU"+str(i), False) + self.player.world.draw.arrow(pos, zvec, 0.2, 2, self.colors[i], "IMU"+str(i), False) + self.player.world.draw.annotation(xvec, "x", Draw.Color.white, "IMU"+str(i), False) + self.player.world.draw.annotation(yvec, "y", Draw.Color.white, "IMU"+str(i), False) + self.player.world.draw.annotation(zvec, "z", Draw.Color.white, "IMU"+str(i), False) + self.player.world.draw.sphere(self.imu_CoM_position[i],0.04,self.colors[i],"IMU"+str(i), True) + + + def compute_local_IMU(self): + r = self.player.world.robot + g = r.gyro / 50 # convert degrees per second to degrees per step + self.imu_torso_to_field_rotation[2].multiply( Matrix_3x3.from_rotation_deg(g), in_place=True, reverse_order=True) + self.imu_torso_position[2][:] = self.imu_torso_next_position[2] + if self.imu_torso_position[2][2] < 0: self.imu_torso_position[2][2] = 0 #limit z coordinate to positive values + self.imu_torso_velocity[2][:] = self.imu_torso_next_velocity[2] + + # convert proper acceleration to coordinate acceleration and fix rounding bias + self.imu_torso_acceleration[2] = self.imu_torso_to_field_rotation[2].multiply(r.acc) + Robot.GRAVITY + self.imu_torso_to_field_transform[2] = Matrix_4x4.from_3x3_and_translation(self.imu_torso_to_field_rotation[2],self.imu_torso_position[2]) + self.imu_head_to_field_transform[2] = self.imu_torso_to_field_transform[2].multiply(r.body_parts["torso"].transform.invert()) + self.imu_CoM_position[2][:] = self.imu_head_to_field_transform[2](r.rel_cart_CoM_position) + + # Next Position = x0 + v0*t + 0.5*a*t^2, Next velocity = v0 + a*t + self.imu_torso_next_position[2] = self.imu_torso_position[2] + self.imu_torso_velocity[2] * 0.02 + self.imu_torso_acceleration[2] * 0.0002 + self.imu_torso_next_velocity[2] = self.imu_torso_velocity[2] + self.imu_torso_acceleration[2] * 0.02 + self.imu_torso_next_velocity[2] *= Robot.IMU_DECAY #stability tradeoff + + def compute_local_IMU_rotation_only(self): + r = self.player.world.robot + g = r.gyro / 50 # convert degrees per second to degrees per step + self.imu_torso_to_field_rotation[1].multiply( Matrix_3x3.from_rotation_deg(g), in_place=True, reverse_order=True) + self.imu_torso_position[1][:] = r.loc_torso_position + self.imu_torso_to_field_transform[1] = Matrix_4x4.from_3x3_and_translation(self.imu_torso_to_field_rotation[1],self.imu_torso_position[1]) + self.imu_head_to_field_transform[1] = self.imu_torso_to_field_transform[1].multiply(r.body_parts["torso"].transform.invert()) + self.imu_CoM_position[1][:] = self.imu_head_to_field_transform[1](r.rel_cart_CoM_position) + + + def update_local_IMU(self, i): + r = self.player.world.robot + self.imu_torso_to_field_rotation[i].m[:] = r.imu_torso_to_field_rotation.m + self.imu_torso_to_field_transform[i].m[:] = r.imu_weak_torso_to_field_transform.m + self.imu_head_to_field_transform[i].m[:] = r.imu_weak_head_to_field_transform.m + self.imu_torso_position[i][:] = r.imu_weak_torso_position + self.imu_torso_velocity[i][:] = r.imu_weak_torso_velocity + self.imu_torso_acceleration[i][:] = r.imu_weak_torso_acceleration + self.imu_torso_next_position[i] = self.imu_torso_position[i] + self.imu_torso_velocity[i] * 0.02 + self.imu_torso_acceleration[i] * 0.0002 + self.imu_torso_next_velocity[i] = self.imu_torso_velocity[i] + self.imu_torso_acceleration[i] * 0.02 + self.imu_CoM_position[i][:] = r.imu_weak_CoM_position + + def execute(self): + + a = self.script.args + self.player = Agent(a.i, a.p, a.m, a.u, a.r, a.t) # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name + + self.player.scom.unofficial_beam((-3,0,self.player.world.robot.beam_height),15) + + for _ in range(10): #beam to place + self.player.scom.commit_and_send() + self.player.scom.receive() + + self.player.world.draw.annotation((-3,1,1.1), "IMU + Localizer", self.colors[0], "note_IMU_1", True) + + for _ in range(150): + self.act() + self.update_local_IMU(0) + self.draw_player_reference_frame(0) + + self.player.world.draw.annotation((-3,1,0.9), "IMU for rotation", self.colors[1], "note_IMU_2", True) + self.update_local_IMU(1) + + for _ in range(200): + self.act() + self.update_local_IMU(0) + self.draw_player_reference_frame(0) + self.compute_local_IMU_rotation_only() + self.draw_player_reference_frame(1) + + self.player.world.draw.annotation((-3,1,0.7), "IMU for rotation & position", self.colors[2], "note_IMU_3", True) + self.update_local_IMU(2) + + for _ in range(200): + self.act() + self.update_local_IMU(0) + self.draw_player_reference_frame(0) + self.compute_local_IMU_rotation_only() + self.draw_player_reference_frame(1) + self.compute_local_IMU() + self.draw_player_reference_frame(2) + + print("\nPress ctrl+c to return.") + + # Still "IMU for rotation & position" but now start walking + self.update_local_IMU(2) + while True: + self.act2() + self.update_local_IMU(0) + self.draw_player_reference_frame(0) + self.compute_local_IMU_rotation_only() + self.draw_player_reference_frame(1) + self.compute_local_IMU() + self.draw_player_reference_frame(2) \ No newline at end of file diff --git a/scripts/utils/Inv_Kinematics.py b/scripts/utils/Inv_Kinematics.py new file mode 100644 index 0000000..235104a --- /dev/null +++ b/scripts/utils/Inv_Kinematics.py @@ -0,0 +1,146 @@ +from agent.Base_Agent import Base_Agent as Agent +from itertools import count +from math_ops.Inverse_Kinematics import Inverse_Kinematics +from scripts.commons.Script import Script +from world.commons.Draw import Draw +import numpy as np + + +class Inv_Kinematics(): + def __init__(self, script:Script) -> None: + self.args = script.args + self.last_action = (0,0,0) + self.gravity = True + + # Initial pose is a neutral pose where all angles are 0 + leg_y_dev, upper_leg_height, upper_leg_depth, lower_leg_len, _, _ = Inverse_Kinematics.NAO_SPECS_PER_ROBOT[self.args.r] + leg_height = upper_leg_height + lower_leg_len + self.feet_pose = [ [[upper_leg_depth,leg_y_dev,-leg_height],[0,0,0]], [[upper_leg_depth,-leg_y_dev,-leg_height], [0,0,0]] ] + + + + def _user_control(self): + + while True: + + inp = input("Command:") + if inp == "": return 2 + elif inp == ".": return 1 + elif inp == "h": self.print_help(); continue + elif inp == "g": + self.gravity = not self.gravity + print("Using gravity:",self.gravity) + if self.gravity: + return 6 # extra steps for beam to take effect + else: + return 1 + + #Check if user input is a value + try: + val = float(inp) + self.feet_pose[self.last_action[0]][self.last_action[1]][self.last_action[2]] = val + continue + except: + pass + + if inp[0] not in ['l','r'] or inp[1] not in ['x','y','z','X','Y','Z']: + print("Illegal command!") + continue + + side = 0 if inp[0]=='l' else 1 + pos_rot = 0 if inp[1].islower() else 1 + axis = {'x':0,'y':1,'z':2}[inp[1].lower()] + self.last_action = (side,pos_rot,axis) + + try: + val = float(inp[2:]) + self.feet_pose[side][pos_rot][axis] = val + except: + print("Illegal value conversion!") + + + + def _draw_labels(self, player:Agent): + r = player.world.robot + robot_pos = r.loc_head_position + for i, body_part in enumerate(['lankle','rankle']): + pos = r.get_body_part_abs_position(body_part) + label_rel_pos = np.array([-0.2,(0.5-i),0]) + label_rel_pos /= np.linalg.norm(label_rel_pos) / 1.0 #labels at 1.0m from body part + player.world.draw.line( pos,pos+label_rel_pos,2,Draw.Color.green_light,body_part,False) + p = self.feet_pose[i] + pose_text = f"x:{p[0][0]:.4f} y:{p[0][1]:.4f} z:{p[0][2]:.4f}", f"rol:{p[1][0]:.2f} (bias) pit:{p[1][1]:.2f} (bias) yaw:{p[1][2]:.2f} " + player.world.draw.annotation( pos+label_rel_pos+[0,0,0.2], pose_text[0], Draw.Color.cyan,body_part,False) + player.world.draw.annotation( pos+label_rel_pos+[0,0,0.1], pose_text[1], Draw.Color.cyan,body_part,False) + + # Draw forward kinematics (ankles positions + feet rotation) + p = player.inv_kinematics.get_body_part_pos_relative_to_hip(body_part) # ankle relative to center of both hip joints + foot_rel_torso = r.head_to_body_part_transform("torso", r.body_parts[['lfoot','rfoot'][i]].transform ) + w = foot_rel_torso.get_roll_deg(), foot_rel_torso.get_pitch_deg(), foot_rel_torso.get_yaw_deg() + pose_text = f"x:{p[0]:.4f} y:{p[1]:.4f} z:{p[2]:.4f}", f"rol:{w[0]:.4f} pit:{w[1]:.4f} yaw:{w[2]:.4f}" + + player.world.draw.annotation( pos+label_rel_pos+[0,0,-0.2], pose_text[0], Draw.Color.red,body_part,False) + player.world.draw.annotation( pos+label_rel_pos+[0,0,-0.3], pose_text[1], Draw.Color.red,body_part,False) + player.world.draw.annotation( pos+label_rel_pos+[0,0,-0.4], "(forward kinematics data)", Draw.Color.red,body_part,True) + + note = f"Torso roll: {r.imu_torso_roll:.2f} Torso pitch: {r.imu_torso_pitch:.2f}" + player.world.draw.annotation( robot_pos+[0,0,0.10],note,Draw.Color.red,"Torso") + + + def print_help(self): + print(""" +---------------- Inverse kinematics demonstration ---------------- +INPUT: ankle positions + feet rotations (relative coordinates) +OUTPUT: angular positions of both legs' joints +------------------------------------------------------------------ +Command: {action/option} + action: [side:{l/r} axis*:{x/y/z/X/Y/Z}] value + *for position use x/y/z, for rotation use X/Y/Z + option: {"",.,g,h} +Examples: + "lz-0.12" - move left ankle to -0.1m in the z-axis + "rX30.5" - rotate right foot to 30.5 deg in the x-axis (roll) + "20" - repeat last action but change value to 20 + "" - advance 2 simulation step + "." - advance 1 simulation step + "g" - toggle gravity + "h" - help, display this message + "ctrl+c" - quit demonstration +------------------------------------------------------------------""") + + def execute(self): + + self.state = 0 + a = self.args + + self.print_help() + player = Agent(a.i, a.p, a.m, a.u, a.r, a.t) # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name + + player.scom.unofficial_beam((-3,0,0.42),0) + + next_control_step = 20 + + for i in count(): + + if self.gravity: + player.scom.unofficial_beam((-3,0,0.42),0) + + self._draw_labels(player) + + if i == next_control_step: + next_control_step += self._user_control() + + for i in range(2): #left and right legs + + indices, values, error_codes = player.inv_kinematics.leg(self.feet_pose[i][0], self.feet_pose[i][1], bool(i==0), False) + + if -1 in error_codes: + print("Position is out of reach!") + error_codes.remove(-1) + for j in error_codes: + print(f"Joint {j} is out of range!") + + player.world.robot.set_joints_target_position_direct(indices,values) + + player.scom.commit_and_send( player.world.robot.get_command() ) + player.scom.receive() \ No newline at end of file diff --git a/scripts/utils/Joints.py b/scripts/utils/Joints.py new file mode 100644 index 0000000..8ca013c --- /dev/null +++ b/scripts/utils/Joints.py @@ -0,0 +1,151 @@ +from agent.Base_Agent import Base_Agent as Agent +from scripts.commons.Script import Script +from world.commons.Draw import Draw +import numpy as np + +class Joints(): + + def __init__(self,script:Script) -> None: + self.script = script + self.agent_pos = (-3,0,0.45) + self.enable_pos = True + self.enable_gravity = False + self.enable_labels = True + self.enable_harmonize = True + self.active_joint = 0 + self.joints_value = None #position or speed + + + def _draw_joints(self,player:Agent): + zstep = 0.05 + label_z = [3*zstep,5*zstep,0,0,zstep,zstep,2*zstep,2*zstep,0,0,0,0,zstep,zstep,0,0,zstep,zstep,4*zstep,4*zstep,5*zstep,5*zstep,0,0] + for j, transf in enumerate(player.world.robot.joints_transform): + rp = transf.get_translation() + pos = player.world.robot.loc_head_to_field_transform(rp,False) + j_id = f"{j}" + j_name = f"{j}" + color = Draw.Color.cyan + if player.world.robot.joints_position[j] != 0: + j_name += f" ({int(player.world.robot.joints_position[j])})" + color = Draw.Color.red + label_rp = np.array([rp[0]-0.0001,rp[1]*0.5,0]) + label_rp /= np.linalg.norm(label_rp) / 0.5 #labels at 0.5m from body part + label_rp += (0,0,label_z[j]) + label = player.world.robot.loc_head_to_field_transform(rp+label_rp,False) + player.world.draw.line( pos,label,2,Draw.Color.green_light,j_id,False) + player.world.draw.annotation( label,j_name,color,j_id) + + + def print_help(self): + print(f""" +---------------------- Joints demonstration ---------------------- +Command: {{action/actions/option}} + action : [joint:{{int}}] value + actions: value0,value1,...,valueN + e.g. if N=10, you control all joints from j0 to j10 + option: {{h,s,g,l,w,r,"",.}} +Examples: + "6 90" - move joint 6 to 90deg or move joint 6 at 90deg/step + "4" - move last joint to 4deg or apply speed of 4deg/step + "1,9,-35"- move joints 0,1,2 to 1deg, 9deg, -35deg (or speed) + "h" - help, display this message + "s" - toggle position/speed control ({"Posi" if self.enable_pos else "Spee"}) + "g" - toggle gravity ({self.enable_gravity}) + "l" - toggle labels ({self.enable_labels}) + "w" - toggle harmonize* ({self.enable_harmonize}) + "r" - reset (position mode + reset joints) + "" - advance 2 simulation step + "." - advance 1 simulation step + "ctrl+c" - quit demonstration + + *all joints end moving at the same time when harmonize is True +------------------------------------------------------------------""") + + def _user_control_step(self,player:Agent): + + while True: + + inp = input("Command: ") + if inp == "s": + self.enable_pos = not self.enable_pos + print("Using", "position" if self.enable_pos else "velocity", "control.") + if self.enable_pos: + self.joints_value[:] = player.world.robot.joints_position + else: + self.joints_value.fill(0) + continue + elif inp == "g": + self.enable_gravity = not self.enable_gravity + print("Using gravity:",self.enable_gravity) + continue + elif inp == "l": + self.enable_labels = not self.enable_labels + print("Using labels:",self.enable_labels) + continue + elif inp == "w": + self.enable_harmonize = not self.enable_harmonize + print("Using harmonize:",self.enable_harmonize) + continue + elif inp == "r": + self.enable_pos = True + self.joints_value.fill(0) + print("Using position control. All joints are set to zero.") + continue + elif inp == "h": + self.print_help(); continue + + elif inp == "": return 1 + elif inp == ".": return 0 + + try: + if " " in inp: + self.active_joint, value = map(float, inp.split()) + self.joints_value[int(self.active_joint)] = value + elif "," in inp: + values = inp.split(",") + self.joints_value[0:len(values)] = values + else: + self.joints_value[self.active_joint] = float(inp) + except: + print("Illegal command!") + continue + + + + def execute(self): + + a = self.script.args + player = Agent(a.i, a.p, a.m, a.u, a.r, a.t) # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name + + self.joints_no = player.world.robot.no_of_joints + self.joints_value = np.zeros(self.joints_no) # initialize + + player.scom.commit_beam(self.agent_pos[0:2],0) + + self.print_help() + + # initialize (+beam) + for _ in range(8): + player.scom.commit_and_send() + player.scom.receive() + self._draw_joints(player) + + skip_next = 0 # variable to advance more than 1 step + + while True: + if skip_next == 0: + skip_next = self._user_control_step(player) + else: + skip_next -= 1 + + if self.enable_labels: + self._draw_joints(player) + + if self.enable_pos: + player.world.robot.set_joints_target_position_direct(slice(self.joints_no), self.joints_value, harmonize=self.enable_harmonize) + else: + player.world.robot.joints_target_speed[:]=self.joints_value * 0.87266463 #deg/step to rad/s + + if not self.enable_gravity: player.scom.unofficial_beam(self.agent_pos,0) + player.scom.commit_and_send( player.world.robot.get_command() ) + player.scom.receive() \ No newline at end of file diff --git a/scripts/utils/Kick.py b/scripts/utils/Kick.py new file mode 100644 index 0000000..2cfe3db --- /dev/null +++ b/scripts/utils/Kick.py @@ -0,0 +1,53 @@ +from agent.Base_Agent import Base_Agent as Agent +from math_ops.Math_Ops import Math_Ops as M +from scripts.commons.Script import Script +import numpy as np + +''' +Objective: +---------- +Demonstrate kick +''' + +class Kick(): + def __init__(self, script:Script) -> None: + self.script = script + + def execute(self): + + a = self.script.args + player = Agent(a.i, a.p, a.m, a.u, a.r, a.t) # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name + player.path_manager.draw_options(enable_obstacles=True, enable_path=True) # enable drawings of obstacles and path to ball + behavior = player.behavior + w = player.world + r = w.robot + + print("\nThe robot will kick towards the center of the field") + print("Try to manually relocate the ball") + print("Press ctrl+c to return\n") + + player.scom.unofficial_set_play_mode("PlayOn") + player.scom.unofficial_beam((-3,0,r.beam_height),0) + vec = (1,0) + + while True: + player.scom.unofficial_set_game_time(0) + b = w.ball_abs_pos[:2] + + if 0 < np.linalg.norm(w.get_ball_abs_vel(6)) < 0.02: # speed of zero is likely to indicate prolongued inability to see the ball + if np.linalg.norm(w.ball_rel_head_cart_pos[:2]) > 0.5: # update kick if ball is further than 0.5 m + if max(abs(b)) < 0.5: + vec = np.array([6,0]) + else: + vec = M.normalize_vec((0,0)-b) * 6 + + w.draw.point(b+vec, 8, w.draw.Color.pink, "target") + + behavior.execute("Basic_Kick", M.vector_angle(vec)) + + player.scom.commit_and_send( r.get_command() ) + player.scom.receive() + + if behavior.is_ready("Get_Up"): + player.scom.unofficial_beam((*r.loc_head_position[0:2],r.beam_height),0) + behavior.execute_to_completion("Zero_Bent_Knees") \ No newline at end of file diff --git a/scripts/utils/Localization.py b/scripts/utils/Localization.py new file mode 100644 index 0000000..bb91937 --- /dev/null +++ b/scripts/utils/Localization.py @@ -0,0 +1,103 @@ +from agent.Agent import Agent as Agent +from cpp.localization import localization +from math_ops.Math_Ops import Math_Ops as M +from scripts.commons.Script import Script +from world.commons.Draw import Draw +from world.commons.Other_Robot import Other_Robot + + +class Localization(): + + def __init__(self,script:Script) -> None: + self.script = script + + + def execute(self): + + a = self.script.args + d = self.draw = Draw(True, 0, a.i, 32769) # using independent draw object so that the internal agent drawings can be disabled + + # Args: Server IP, Agent Port, Monitor Port, Uniform No., Team name, Enable Log, Enable Draw + self.script.batch_create(Agent, ((a.i,a.p,a.m,1,a.t,False,False),)) # one teammate (dummy goalkeeper without communication) + self.script.batch_create(Agent, ((a.i,a.p,a.m,5,"Opponent",False,False),)) # one opponent + self.script.batch_create(Agent, ((a.i,a.p,a.m,9,a.t,False,False),)) # one main agent (the one who draws its world) + + # Beam dummy goalkeeper + self.script.batch_unofficial_beam( ((-14,0,0.5,0),), slice(0,1)) + + p : Agent = self.script.players[-1] # p identifies the main agent + p.scom.unofficial_set_play_mode("PlayOn") + + # Execute + while True: + self.script.batch_commit_and_send(slice(0,1)) # dummy agent does not think + self.script.batch_execute_agent(slice(1,None)) # execute normal agents + self.script.batch_receive(slice(0,1), False) # receive & don't update dummy's world state (to save cpu resources) + self.script.batch_receive(slice(1,None)) # receive & update world state + + if p.world.vision_is_up_to_date: + if p.world.robot.loc_is_up_to_date: # localization will draw the world of the last agent to be executed + localization.print_python_data() # print data received by the localization module + localization.draw_visible_elements(not p.world.team_side_is_left) # draw visible elements + localization.print_report() # print report with stats + print("\nPress ctrl+c to return.") + d.circle( p.world.ball_abs_pos, 0.1,6,Draw.Color.purple_magenta,"world", False) + else: + d.annotation( p.world.robot.cheat_abs_pos, "Not enough visual data!", Draw.Color.red,"world", False) + + for o in p.world.teammates: + if o.state_last_update != 0 and not o.is_self: # skip if other robot was not yet seen + self._draw_other_robot(p, o, Draw.Color.white) + + for o in p.world.opponents: + if o.state_last_update != 0: # skip if other robot was not yet seen + self._draw_other_robot(p, o, Draw.Color.red) + + d.flush("world") + + + + def _draw_other_robot(self, p:Agent, o:Other_Robot, team_color): + #p - player that sees + #o - other robot (player that is seen) + + d = self.draw + white = Draw.Color.white + green = Draw.Color.green_light + gray = Draw.Color.gray_20 + + time_diff = p.world.time_local_ms - o.state_last_update + if time_diff > 0: + white = Draw.Color.gray_40 + green = Draw.Color.get(107, 139, 107) + gray = Draw.Color.gray_50 + + #orientation + if len(o.state_abs_pos)==3: + line_tip = o.state_abs_pos + (0.5*M.deg_cos(o.state_orientation),0.5*M.deg_sin(o.state_orientation),0) + d.line( o.state_abs_pos, line_tip, 3, white, "world", False) + else: + temp_pos = M.to_3d(o.state_abs_pos, 0.3) + line_tip = temp_pos + (0.5*M.deg_cos(o.state_orientation),0.5*M.deg_sin(o.state_orientation),0) + d.line( temp_pos, line_tip, 3, Draw.Color.yellow, "world", False) + + #body parts + for pos in o.state_body_parts_abs_pos.values(): + d.sphere( pos, 0.07, green,"world", False) + + #player ground area + d.circle( o.state_ground_area[0], o.state_ground_area[1], 6, team_color,"world", False) + + #distance + midpoint = (o.state_abs_pos[0:2] + p.world.robot.loc_head_position[0:2])/2 + d.line( o.state_abs_pos[0:2], p.world.robot.loc_head_position[0:2], 1, gray, "world", False) + d.annotation( midpoint, f'{o.state_horizontal_dist:.2f}m', white, "world", False) + + #velocity + arrow_tip = o.state_abs_pos[0:2] + o.state_filtered_velocity[0:2] + d.arrow( o.state_abs_pos[0:2], arrow_tip, 0.2, 4, green, "world", False) + + #state + state_color = white if not o.state_fallen else Draw.Color.yellow + d.annotation( (o.state_abs_pos[0],o.state_abs_pos[1],1), + f"({o.unum}) {'Fallen' if o.state_fallen else 'Normal'}", state_color, "world", False) \ No newline at end of file diff --git a/scripts/utils/Pathfinding.py b/scripts/utils/Pathfinding.py new file mode 100644 index 0000000..c371b19 --- /dev/null +++ b/scripts/utils/Pathfinding.py @@ -0,0 +1,203 @@ +from agent.Base_Agent import Base_Agent as Agent +from cpp.a_star import a_star +from scripts.commons.Script import Script +import numpy as np +import time + +''' +:::::::::::::::::::::::::::::::::::::::::: +::::::::a_star.compute(param_vec)::::::::: +:::::::::::::::::::::::::::::::::::::::::: + +param_vec (numpy array, float32) +param_vec[0] - start x +param_vec[1] - start y +param_vec[2] - allow path to go out of bounds? (useful when player does not have the ball) +param_vec[3] - go to opposite goal? (path goes to the most efficient part of the goal) +param_vec[4] - target x (only used if param_vec[3]==0) +param_vec[5] - target y (only used if param_vec[3]==0) +param_vec[6] - timeout in us (maximum execution time) +-------------- [optional] ---------------- +param_vec[ 7-11] - obstacle 1: x, y, hard radius (max:5m), soft radius (max:5m), repulsive force for soft radius (min:0) +param_vec[12-16] - obstacle 2: x, y, hard radius (max:5m), soft radius (max:5m), repulsive force for soft radius (min:0) +... - obstacle n: x, y, hard radius (max:5m), soft radius (max:5m), repulsive force for soft radius (min:0) +---------------- return ------------------ +path_ret : numpy array (float32) + path_ret[:-2] + contains path from start to target (up to a maximum of 1024 positions) + each position is composed of x,y coordinates (so, up to 2048 coordinates) + the return vector is flat (1 dimension) (e.g. [x1,y1,x2,y2,x3,y3,...]) + reasons why path may not end in target: + - path is longer than 1024 positions (which is at least 102 meters!) + - reaching target is impossible or timeout (in which case, the path ends in the closest position to target found) + path_ret[-2] + number indicating the path status + 0 - success + 1 - timeout before the target was reached (may be impossible) + 2 - impossible to reach target (all options were tested) + 3 - no obstacles between start and target (path_ret[:-2] contains only 2 points: the start and target) + path_ret[-1] + A* path cost +:::::::::::::::::::::::::::::::::::::::::: +::::::::::::::::::Notes::::::::::::::::::: +:::::::::::::::::::::::::::::::::::::::::: + +Map of field: + - The algorithm has a 32m by 22m map with a precision of 10cm (same dimension as field +1 meter border) + - The map contains information about field lines, goal posts and goal net + - The path may go outside the field (out of bounds) if the user allows it, + but it may never go through goal posts or the goal net (these are considered static inaccessible obstacles) + - The user must only specify dynamic obstacles through the arguments + +Repulsive force: + - The repulsive force is implemented as an extra cost for the A* algorithm + - The cost for walking 10cm is 1, and the cost for walking diagonally is sqrt(2) + - The extra cost of stepping on a position with a repulsive force f=1 is 1 + - For any given position on the field, the repulsive force of >=2 objects is combined with the max function, max(f1,f2), NOT f1+f2! + - If path starts on inaccessible position, it can go to a neighbor inaccessible position but there is a cost of 100 (to avoid inaccessible paths) + Example: + Map 1 Map 2 Map 3 + ..x.. ..o.. ..o.. + ..1.. ..o.. .o1.. + ..o.. ..o.. ..o.. + Consider 'Map 1' where 'x' is the target, 'o' is the player, and '1' is a repulsive force of 1 + In 'Map 2', the player chooses to go forward, the total cost of this path is: 1+(extra=1)+1 = 3 + In 'Map 3', the player avoids the repulsive force, the total cost of this path is: sqrt(2)+sqrt(2) = 2.83 (optimal solution) + Map 1 Map 2 Map 3 Map 4 + ...x... ..oo... ...o... ...o... + ..123.. .o123.. ..o23.. ..1o3.. + ...o... ..oo... ...o... ...o... + Consider 'Map 1' with 3 positions with 3 distinct repulsive forces, going from 1 to 3. + In 'Map 2', the player avoids all repulsive forces, total cost: 1+sqrt(2)+sqrt(2)+1 = 4.83 + In 'Map 3', the player goes through the smallest repulsive force, total cost: sqrt(2)+(extra=1)+sqrt(2) = 3.83 (optimal solution) + In 'Map 4', the player chooses to go forward, total cost: 1+(extra=2)+1 = 4.00 + +Obstacles: + hard radius: inaccessible obstacle radius (infinite repulsive force) + soft radius: accessible obstacle radius with user-defined repulsive force (fades with distance) (disabled if <= hard radius) + Example: + obstacle(0,0,1,3,5) -> obstacle at pos(0,0) with hard radius of 1m, soft radius of 3m with repulsive force 5 + - the path cannot be at <=1m from this obstacle, unless the path were to start inside that radius + - the soft radius force is maximum at the center (5) and fades with distance until (0) at 3m from the obstacle + - so to sum up, at a distance of [0,1]m the force is infinite, [1,3]m the force goes from 3.333 to 0 + obstacle(-2.1,3,0,0,0) -> obstacle at pos(-2.1,3) with hard radius of 0m, soft radius of 0m with repulsive force 0 + - the path cannot go through (-2.1,3) + obstacle(-2.16,3,0,0,8) -> obstacle at pos(-2.2,3) with hard radius of 0m, soft radius of 0m with repulsive force 8 + - the path cannot go through (-2.2,3), the map has a precision of 10cm, so the obstacle is placed at the nearest valid position + - the repulsive force is ignored because (soft radius <= hard radius) +''' + + + +class Pathfinding(): + def __init__(self, script:Script) -> None: + self.script = script + a_star.compute(np.zeros(6, np.float32)) # Initialize (not needed, but the first run takes a bit more time) + + def draw_grid(self): + d = self.player.world.draw + MAX_RAW_COST = 0.6 # dribble cushion + + for x in np.arange(-16,16.01,0.1): + for y in np.arange(-11,11.01,0.1): + s_in, cost_in = a_star.compute(np.array([x, y, 0, 0, x, y, 5000], np.float32))[-2:] # do not allow out of bounds + s_out, cost_out = a_star.compute(np.array([x, y, 1, 0, x, y, 5000], np.float32))[-2:] # allow out of bounds + #print(path_cost_in, path_cost_out) + if s_out != 3: + d.point((x,y), 5, d.Color.red, "grid", False) + elif s_in != 3: + d.point((x,y), 4, d.Color.blue_pale, "grid", False) + elif 0 < cost_in < MAX_RAW_COST + 1e-6: + d.point((x,y), 4, d.Color.get(255,(1-cost_in/MAX_RAW_COST)*255,0), "grid", False) + elif cost_in > MAX_RAW_COST: + d.point((x,y), 4, d.Color.black, "grid", False) + #else: + # d.point((x,y), 4, d.Color.white, "grid", False) + d.flush("grid") + + def sync(self): + r = self.player.world.robot + self.player.behavior.head.execute() + self.player.scom.commit_and_send( r.get_command() ) + self.player.scom.receive() + + def draw_path_and_obstacles(self, obst, path_ret_pb, path_ret_bp): + w = self.player.world + + # draw obstacles + for i in range(0,len(obst[0]),5): + w.draw.circle(obst[0][i:i+2], obst[0][i+2], 2, w.draw.Color.red, "obstacles", False) + w.draw.circle(obst[0][i:i+2], obst[0][i+3], 2, w.draw.Color.orange, "obstacles", False) + + # draw path + path_pb = path_ret_pb[:-2] # create view without status + path_status_pb = path_ret_pb[-2] # extract status + path_cost_pb = path_ret_pb[-1] # extract A* cost + path_bp = path_ret_bp[:-2] # create view without status + path_status_bp = path_ret_bp[-2] # extract status + path_cost_bp = path_ret_bp[-1] # extract A* cost + + c_pb = {0: w.draw.Color.green_lime, 1: w.draw.Color.yellow, 2: w.draw.Color.red, 3: w.draw.Color.blue_light}[path_status_pb] + c_bp = {0: w.draw.Color.green_pale, 1: w.draw.Color.yellow_light, 2: w.draw.Color.red_salmon, 3: w.draw.Color.blue_pale}[path_status_bp] + + for i in range(2,len(path_pb)-2,2): + w.draw.line(path_pb[i-2:i],path_pb[i:i+2], 5, c_pb, "path_player_ball", False) + + if len(path_pb)>=4: + w.draw.arrow(path_pb[-4:-2],path_pb[-2:],0.4, 5, c_pb, "path_player_ball", False) + + for i in range(2,len(path_bp)-2,2): + w.draw.line(path_bp[i-2:i],path_bp[i:i+2], 5, c_bp, "path_ball_player", False) + + if len(path_bp)>=4: + w.draw.arrow(path_bp[-4:-2],path_bp[-2:],0.4, 5, c_bp, "path_ball_player", False) + + w.draw.flush("obstacles") + w.draw.flush("path_player_ball") + w.draw.flush("path_ball_player") + + def move_obstacles(self, obst): + + for i in range(len(obst[0])//5): + obst[0][i*5] +=obst[1][i,0] + obst[0][i*5+1]+=obst[1][i,1] + if not -16ball) + param_vec_bp = np.array([*ball, 0, go_to_goal, *rpos, timeout, *obst[0]], np.float32) # don't allow (ball->player) + t1 = time.time() + path_ret_pb = a_star.compute(param_vec_pb) + t2 = time.time() + path_ret_bp = a_star.compute(param_vec_bp) + t3 = time.time() + + print(end=f"\rplayer->ball {int((t2-t1)*1000000):5}us (len:{len(path_ret_pb[:-2])//2:4}) ball->player {int((t3-t2)*1000000):5}us (len:{len(path_ret_bp[:-2])//2:4}) ") + + self.draw_path_and_obstacles( obst, path_ret_pb, path_ret_bp ) + self.sync() diff --git a/scripts/utils/Radio_Localization.py b/scripts/utils/Radio_Localization.py new file mode 100644 index 0000000..ccdca6f --- /dev/null +++ b/scripts/utils/Radio_Localization.py @@ -0,0 +1,97 @@ +from agent.Agent import Agent +from itertools import count +from scripts.commons.Script import Script +from typing import List +from world.commons.Draw import Draw + +class Radio_Localization(): + def __init__(self,script:Script) -> None: + self.script = script + + def draw_objects(self, p:Agent, pos, is_down, was_seen, last_update, is_self=False): + w = p.world + me = w.robot.loc_head_position + + # get draw object from same player to always overwrite previous drawing + # could also use team channel but with this approach we could draw for both teams + d:Draw = self.script.players[0].world.draw + + # VISUALSTEP_MS is the time it takes to get a visual update + is_current = last_update > w.time_local_ms - w.VISUALSTEP_MS + + # 0.12s is the time it takes to do a full broadcast with all positions if every group is completely visible + # here we use >= instead of > because the radio message comes with a delay of 20ms + is_recent = last_update >= w.time_local_ms - 120 + + if is_current and was_seen: + c = d.Color.green_light # I've seen this object in the current or previous time step + elif is_recent and was_seen: + c = d.Color.green # I've seen this object in the last 0.12s + elif is_current: + c = d.Color.yellow # I've heard about this object in the current or previous time step (and it was not seen in the same period) + elif is_recent: + c = d.Color.yellow_light # I've heard about this object in the last 0.12s (the last available info was not obtained from vision) + else: + c = d.Color.red # I haven't seen or heard about this object in the last 0.12s + + if is_self: + if w.robot.radio_fallen_state: + d.annotation(me, "Fallen (radio)", d.Color.yellow, "objects", False) # I heard I've fallen (but I missed the last 2 visual steps) + elif w.robot.loc_head_z < 0.3: + d.annotation(me, "Fallen (internal)", d.Color.white, "objects", False) # I have detected I've fallen + d.sphere(me, 0.06, c, "objects", False) + else: + if is_down: + d.annotation((me[:2]+pos[:2])/2,"Fallen",d.Color.white,"objects",False) + d.arrow(me, pos, 0.1, 3, c, "objects", False) + + + def draw(self,p:Agent): + w = p.world + others = w.teammates + w.opponents + + #----------------------------------------------------------- draw other players + + for o in others: + if o.is_self or o.state_last_update==0: # do not draw self or never before seen players + continue + + pos = o.state_abs_pos + is_down = o.state_fallen + # 3D head position means head is visible, 2D means some body parts are visible but not the head, or the head position comes from radio + is_3D = pos is not None and len(pos)==3 + + self.draw_objects(p, pos, is_down, is_3D, o.state_last_update) + + #----------------------------------------------------------- draw self + + is_pos_from_vision = w.robot.loc_head_position_last_update == w.robot.loc_last_update + self.draw_objects(p, None, None, is_pos_from_vision, w.robot.loc_head_position_last_update, True) + + #----------------------------------------------------------- draw ball and flush drawings + + self.draw_objects(p, w.ball_abs_pos, False, w.is_ball_abs_pos_from_vision, w.ball_abs_pos_last_update) + self.script.players[0].world.draw.flush("objects") + + + def execute(self): + a = self.script.args + + # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name, Enable Log, Enable Draw + self.script.batch_create(Agent, ((a.i,a.p,a.m,u,a.t, False,u==1) for u in range(1,12))) + self.script.batch_create(Agent, ((a.i,a.p,a.m,u,"Opponent",False,False) for u in range(1,12))) + players : List[Agent] = self.script.players + + # Beam opponents + beam_pos = [(-(i//2)-3,(i%2*2-1)*(i//2+1),0) for i in range(11)] + self.script.batch_commit_beam( beam_pos, slice(11,None) ) + print("\nPress ctrl+c to return.") + + # Execute + for j in count(): + self.script.batch_execute_agent( slice(11) ) # run our agents (think and send) + self.script.batch_commit_and_send( slice(11,None) ) # run their agents (don't think, just send) + + self.draw(players[j//15%11]) # draw knowledge, iterate through our team, 15 time steps per player + self.script.batch_receive(slice(11)) # receive & update our team's world state + self.script.batch_receive(slice(11,None), False) # receive & don't update opponent's world state (to save cpu resources) diff --git a/scripts/utils/Server.py b/scripts/utils/Server.py new file mode 100644 index 0000000..47dc714 --- /dev/null +++ b/scripts/utils/Server.py @@ -0,0 +1,148 @@ +import os + +class Server(): + + def __init__(self,script) -> None: + + if os.path.isdir("/usr/local/share/rcssserver3d/"): + self.source = "/usr/local/share/rcssserver3d/" + elif os.path.isdir("/usr/share/rcssserver3d/"): + self.source = "/usr/share/rcssserver3d/" + else: + raise FileNotFoundError("The server configuration files were not found!") + + # To add options: insert into options & explations with same index, read respective value from file or from other values, add edit entry + + self.options = ["Official Config", "Penalty Shootout", "Soccer Rules", "Sync Mode", "Real Time", "Cheats", "Full Vision", "Add Noise", "25Hz Monitor"] + self.descriptions = ["Configuration used in official matches", "Server's Penalty Shootout mode", "Play modes, automatic referee, etc.", + "Synchronous communication between agents and server", "Real Time (or maximum server speed)", + "Agent position & orientation, ball position", "See 360 deg instead of 120 deg (vertically & horizontally)", + "Noise added to the position of visible objects", "25Hz Monitor (or 50Hz but RoboViz will show 2x the actual speed)"] + + spark_f = os.path.expanduser("~/.simspark/spark.rb") + naoneckhead_f = self.source+"rsg/agent/nao/naoneckhead.rsg" + + self.files = {"Penalty Shootout" : self.source + "naosoccersim.rb", + "Soccer Rules" : self.source + "naosoccersim.rb", + "Sync Mode" : spark_f, + "Real Time" : self.source+"rcssserver3d.rb", + "Cheats" : naoneckhead_f, + "Full Vision" : naoneckhead_f, + "Add Noise" : naoneckhead_f, + "25Hz Monitor" : spark_f} + + + def label(self, setting_name, t_on, t_off): + with open(self.files[setting_name], "r") as sources: + content = sources.read() + + if t_on in content: + self.values[setting_name] = "On" + elif t_off in content: + self.values[setting_name] = "Off" + else: + self.values[setting_name] = "Error" + + + def read_config(self): + v = self.values = dict() + + print("Reading server configuration files...") + + self.label("Penalty Shootout", "addSoccerVar('PenaltyShootout', true)", "addSoccerVar('PenaltyShootout', false)") + self.label("Soccer Rules", " gameControlServer.initControlAspect('SoccerRuleAspect')", "#gameControlServer.initControlAspect('SoccerRuleAspect')") + self.label("Real Time", "enableRealTimeMode = true", "enableRealTimeMode = false") + self.label("Cheats", "setSenseMyPos true", "setSenseMyPos false") + self.label("Full Vision", "setViewCones 360 360", "setViewCones 120 120") + self.label("Add Noise", "addNoise true", "addNoise false") + self.label("Sync Mode", "agentSyncMode = true", "agentSyncMode = false") + self.label("25Hz Monitor", "monitorStep = 0.04", "monitorStep = 0.02") + + is_official_config = (v["Penalty Shootout"] == "Off" and v["Soccer Rules"] == "On" and v["Real Time"] == "On" and v["Cheats"] == "Off" and v["Full Vision"] == "Off" and + v["Add Noise"] == "On" and v["Sync Mode"] == "Off" and v["25Hz Monitor"] == "On") + print(v["Penalty Shootout"], is_official_config) + v["Official Config"] = "On" if is_official_config else "Off" + + def change_config(self, setting_name, t_on, t_off, current_value=None, file=None): + if current_value is None: + current_value = self.values[setting_name] + + if file is None: + file = self.files[setting_name] + + with open(file, "r") as sources: + t = sources.read() + if current_value == "On": + t = t.replace(t_on, t_off, 1) + print(f"Replacing '{t_on}' with '{t_off}' in '{file}'") + elif current_value == "Off": + t = t.replace(t_off, t_on, 1) + print(f"Replacing '{t_off}' with '{t_on}' in '{file}'") + else: + print(setting_name, "was not changed because the value is unknown!") + with open(file, "w") as sources: + sources.write(t) + + def execute(self): + + while True: + self.read_config() + + # Convert convenient values dict to list + values_list = [self.values[o] for o in self.options] + + print() + UI.print_table( [self.options, values_list, self.descriptions], ["Setting", "Value", "Description"], numbering=[True, False, False]) + choice = UI.read_int('Choose setting (ctrl+c to return): ',0,len(self.options)) + opt = self.options[choice] + + prefix = ['sudo', 'python3', 'scripts/utils/Server.py', opt] + + if opt in self.files: + suffix = [self.values[opt], self.files[opt]] + + if opt == "Penalty Shootout": + subprocess.call([*prefix, "addSoccerVar('PenaltyShootout', true)", "addSoccerVar('PenaltyShootout', false)", *suffix]) + elif opt == "Soccer Rules": + subprocess.call([*prefix, "gameControlServer.initControlAspect('SoccerRuleAspect')", "#gameControlServer.initControlAspect('SoccerRuleAspect')", *suffix]) + elif opt == "Sync Mode": + self.change_config(opt, "agentSyncMode = true", "agentSyncMode = false") # doesn't need sudo privileges + elif opt == "Real Time": + subprocess.call([*prefix, "enableRealTimeMode = true", "enableRealTimeMode = false", *suffix]) + elif opt == "Cheats": + subprocess.call([*prefix, "setSenseMyPos true", "setSenseMyPos false", *suffix, + opt, "setSenseMyOrien true", "setSenseMyOrien false", *suffix, + opt, "setSenseBallPos true", "setSenseBallPos false", *suffix]) + elif opt == "Full Vision": + subprocess.call([*prefix, "setViewCones 360 360", "setViewCones 120 120", *suffix]) + elif opt == "Add Noise": + subprocess.call([*prefix, "addNoise true", "addNoise false", *suffix]) + elif opt == "25Hz Monitor": + self.change_config(opt, "monitorStep = 0.04", "monitorStep = 0.02") # doesn't need sudo privileges + elif opt == "Official Config": + if self.values[opt] == "On": + print("The official configuration is already On!") + else: # here, the first option is the official value + subprocess.call([*prefix[:3], + "Penalty Shootout", "addSoccerVar('PenaltyShootout', false)", "addSoccerVar('PenaltyShootout', true)", "Off", self.files["Penalty Shootout"], + "Soccer Rules", "gameControlServer.initControlAspect('SoccerRuleAspect')", "#gameControlServer.initControlAspect('SoccerRuleAspect')", "Off", self.files["Soccer Rules"], + "Sync Mode", "agentSyncMode = false", "agentSyncMode = true", "Off", self.files["Sync Mode"], + "Real Time", "enableRealTimeMode = true", "enableRealTimeMode = false", "Off", self.files["Real Time"], + "Cheats", "setSenseMyPos false", "setSenseMyPos true", "Off", self.files["Cheats"], + "Cheats", "setSenseMyOrien false", "setSenseMyOrien true", "Off", self.files["Cheats"], + "Cheats", "setSenseBallPos false", "setSenseBallPos true", "Off", self.files["Cheats"], + "Full Vision", "setViewCones 120 120", "setViewCones 360 360", "Off", self.files["Full Vision"], + "Add Noise", "addNoise true", "addNoise false", "Off", self.files["Add Noise"], + "25Hz Monitor", "monitorStep = 0.04", "monitorStep = 0.02", "Off", self.files["25Hz Monitor"]]) + + +# process with sudo privileges to change the configuration files +if __name__ == "__main__": + import sys + s = Server(None) + + for i in range(1,len(sys.argv),5): + s.change_config( *sys.argv[i:i+5] ) +else: + import subprocess + from scripts.commons.UI import UI diff --git a/scripts/utils/Team_Communication.py b/scripts/utils/Team_Communication.py new file mode 100644 index 0000000..064a42c --- /dev/null +++ b/scripts/utils/Team_Communication.py @@ -0,0 +1,73 @@ +from agent.Base_Agent import Base_Agent as Agent +from itertools import count +from scripts.commons.Script import Script + +''' +How does communication work? + The say command allows a player to broadcast a message to everyone on the field + Message range: 50m (the field is 36m diagonally, so ignore this limitation) + The hear perceptor indicates 3 things: + - the message + - the origin team + - the origin absolute angle (set to "self" if the message was sent by oneself) + + Messages are heard in the next step. + Messages are only sent every 2 steps (0.04s). + Messages sent in muted steps are only heard by oneself. + In one time step, a player can only hear one other player besides itself. + If two other players say something, only the first message is heard. + This ability exists independetly for messages from both teams. + In theory, a player can hear its own message + the 1st teammate to speak + the 1st opponent to speak + In practice, the opponent doesn't matter because our team's parser ignores messages from other teams + + Message characteristics: + Maximum 20 characters, ascii between 0x20, 0x7E except ' ', '(', ')' + Accepted: letters+numbers+symbols: !"#$%&'*+,-./:;<=>?@[\]^_`{|}~ + However, due to a server bug, sending ' or " ends the message sooner + +''' + +class Team_Communication(): + + def __init__(self,script:Script) -> None: + self.script = script + + def player1_hear(self, msg:bytes, direction, timestamp:float) -> None: + print(f"Player 1 heard: {msg.decode():20} from:{direction:7} timestamp:{timestamp}") + + def player2_hear(self, msg:bytes, direction, timestamp:float) -> None: + print(f"Player 2 heard: {msg.decode():20} from:{direction:7} timestamp:{timestamp}") + + def player3_hear(self, msg:bytes, direction, timestamp:float) -> None: + print(f"Player 3 heard: {msg.decode():20} from:{direction:7} timestamp:{timestamp}") + + + def execute(self): + + a = self.script.args + + hear_callbacks = (self.player1_hear, self.player2_hear, self.player3_hear) + + # Args: Server IP, Agent Port, Monitor Port, Uniform No., Robot Type, Team Name, Enable Log, Enable Draw, Play Mode Correction, Wait for Server, Hear Callback + self.script.batch_create(Agent, ((a.i,a.p,a.m,i+1,0,a.t,True,True,False,True,clbk) for i,clbk in enumerate(hear_callbacks))) + p1:Agent = self.script.players[0] + p2:Agent = self.script.players[1] + p3:Agent = self.script.players[2] + + # Beam players + self.script.batch_commit_beam( [(-2,i,45) for i in range(3)] ) + + for i in count(): + msg1 = b"I_am_p1!_no:"+str(i).encode() + msg2 = b"I_am_p2!_no:"+str(i).encode() + msg3 = b"I_am_p3!_no:"+str(i).encode() + p1.scom.commit_announcement(msg1) # commit message + p2.scom.commit_announcement(msg2) # commit message + p3.scom.commit_announcement(msg3) # commit message + self.script.batch_commit_and_send() # send message + print(f"Player 1 sent: {msg1.decode()} HEX: {' '.join([f'{m:02X}' for m in msg1])}") + print(f"Player 2 sent: {msg2.decode()} HEX: {' '.join([f'{m:02X}' for m in msg2])}") + print(f"Player 3 sent: {msg3.decode()} HEX: {' '.join([f'{m:02X}' for m in msg3])}") + self.script.batch_receive() + input("Press enter to continue or ctrl+c to return.") + diff --git a/start.sh b/start.sh new file mode 100755 index 0000000..48ba4b3 --- /dev/null +++ b/start.sh @@ -0,0 +1,9 @@ +#!/bin/bash +export OMP_NUM_THREADS=1 + +host=${1:-localhost} +port=${2:-3100} + +for i in {1..11}; do + python3 ./Run_Player.py -i $host -p $port -u $i -t FCPortugal -P 0 -D 0 & +done \ No newline at end of file diff --git a/start_debug.sh b/start_debug.sh new file mode 100755 index 0000000..e98c7bc --- /dev/null +++ b/start_debug.sh @@ -0,0 +1,9 @@ +#!/bin/bash +export OMP_NUM_THREADS=1 + +host=${1:-localhost} +port=${2:-3100} + +for i in {1..11}; do + python3 ./Run_Player.py -i $host -p $port -u $i -t FCP-debug -P 0 -D 1 & +done \ No newline at end of file diff --git a/start_fat_proxy.sh b/start_fat_proxy.sh new file mode 100755 index 0000000..cd86dd3 --- /dev/null +++ b/start_fat_proxy.sh @@ -0,0 +1,9 @@ +#!/bin/bash +export OMP_NUM_THREADS=1 + +host=${1:-localhost} +port=${2:-3100} + +for i in {1..11}; do + python3 ./Run_Player.py -i $host -p $port -u $i -t FCPortugal -F 1 -D 0 & +done \ No newline at end of file diff --git a/start_fat_proxy_debug.sh b/start_fat_proxy_debug.sh new file mode 100755 index 0000000..c156595 --- /dev/null +++ b/start_fat_proxy_debug.sh @@ -0,0 +1,9 @@ +#!/bin/bash +export OMP_NUM_THREADS=1 + +host=${1:-localhost} +port=${2:-3100} + +for i in {1..11}; do + python3 ./Run_Player.py -i $host -p $port -u $i -t FCP-debug -F 1 -D 1 & +done \ No newline at end of file diff --git a/start_penalty.sh b/start_penalty.sh new file mode 100755 index 0000000..f430951 --- /dev/null +++ b/start_penalty.sh @@ -0,0 +1,8 @@ +#!/bin/bash +export OMP_NUM_THREADS=1 + +host=${1:-localhost} +port=${2:-3100} + +python3 ./Run_Player.py -i $host -p $port -u 1 -t FCPortugal -P 1 -D 0 & +python3 ./Run_Player.py -i $host -p $port -u 11 -t FCPortugal -P 1 -D 0 & \ No newline at end of file diff --git a/start_penalty_debug.sh b/start_penalty_debug.sh new file mode 100755 index 0000000..002353e --- /dev/null +++ b/start_penalty_debug.sh @@ -0,0 +1,8 @@ +#!/bin/bash +export OMP_NUM_THREADS=1 + +host=${1:-localhost} +port=${2:-3100} + +python3 ./Run_Player.py -i $host -p $port -u 1 -t FCP-debug -P 1 -D 1 & +python3 ./Run_Player.py -i $host -p $port -u 11 -t FCP-debug -P 1 -D 1 & \ No newline at end of file diff --git a/world/Robot.py b/world/Robot.py new file mode 100644 index 0000000..bdeb901 --- /dev/null +++ b/world/Robot.py @@ -0,0 +1,535 @@ +from collections import deque +from math import atan, pi, sqrt, tan +from math_ops.Math_Ops import Math_Ops as M +from math_ops.Matrix_3x3 import Matrix_3x3 +from math_ops.Matrix_4x4 import Matrix_4x4 +from world.commons.Body_Part import Body_Part +from world.commons.Joint_Info import Joint_Info +import numpy as np +import xml.etree.ElementTree as xmlp + +class Robot(): + STEPTIME = 0.02 # Fixed step time + VISUALSTEP = 0.04 # Fixed visual step time + SQ_STEPTIME = STEPTIME * STEPTIME + GRAVITY = np.array([0,0,-9.81]) + IMU_DECAY = 0.996 #IMU's velocity decay + + #------------------ constants to force symmetry in joints/effectors + + MAP_PERCEPTOR_TO_INDEX = {"hj1":0, "hj2":1, "llj1":2, "rlj1":3, + "llj2":4, "rlj2":5, "llj3":6, "rlj3":7, + "llj4":8, "rlj4":9, "llj5":10,"rlj5":11, + "llj6":12,"rlj6":13,"laj1":14,"raj1":15, + "laj2":16,"raj2":17,"laj3":18,"raj3":19, + "laj4":20,"raj4":21,"llj7":22,"rlj7":23 } + + # Fix symmetry issues 1a/4 (identification) + FIX_PERCEPTOR_SET = {'rlj2','rlj6','raj2','laj3','laj4'} + FIX_INDICES_LIST = [5,13,17,18,20] + + # Recommended height for unofficial beam (near ground) + BEAM_HEIGHTS = [0.4, 0.43, 0.4, 0.46, 0.4] + + + def __init__(self, unum:int, robot_type:int) -> None: + robot_xml = "nao"+str(robot_type)+".xml" # Typical NAO file name + self.type = robot_type + self.beam_height = Robot.BEAM_HEIGHTS[robot_type] + self.no_of_joints = 24 if robot_type == 4 else 22 + + #Fix symmetry issues 1b/4 (identification) + self.FIX_EFFECTOR_MASK = np.ones(self.no_of_joints) + self.FIX_EFFECTOR_MASK[Robot.FIX_INDICES_LIST] = -1 + + self.body_parts = dict() # keys='body part names' (given by the robot's XML), values='Body_Part objects' + self.unum = unum # Robot's uniform number + self.gyro = np.zeros(3) # Angular velocity along the three axes of freedom of the robot's torso (deg/s) + self.acc = np.zeros(3) # Proper acceleration along the three axes of freedom of the robot's torso (m/s2) + self.frp = dict() # foot "lf"/"rf", toe "lf1"/"rf1" resistance perceptor (relative [p]oint of origin + [f]orce vector) e.g. {"lf":(px,py,pz,fx,fy,fz)} + self.feet_toes_last_touch = {"lf":0,"rf":0,"lf1":0,"rf1":0} # foot "lf"/"rf", toe "lf1"/"rf1" World.time_local_ms when foot/toe last touched any surface + self.feet_toes_are_touching = {"lf":False,"rf":False,"lf1":False,"rf1":False} # foot "lf"/"rf", toe "lf1"/"rf1" True if touching in last received server message + self.fwd_kinematics_list = None # List of body parts, ordered according to dependencies + self.rel_cart_CoM_position = np.zeros(3) # Center of Mass position, relative to head, in cartesian coordinates (m) + + # Joint variables are optimized for performance / array operations + self.joints_position = np.zeros(self.no_of_joints) # Joints' angular position (deg) + self.joints_speed = np.zeros(self.no_of_joints) # Joints' angular speed (rad/s) + self.joints_target_speed = np.zeros(self.no_of_joints) # Joints' target speed (rad/s) (max: 6.1395 rad/s, see rcssserver3d/data/rsg/agent/nao/hingejoint.rsg) + self.joints_target_last_speed = np.zeros(self.no_of_joints) # Joints' last target speed (rad/s) (max: 6.1395 rad/s, see rcssserver3d/data/rsg/agent/nao/hingejoint.rsg) + self.joints_info = [None] * self.no_of_joints # Joints' constant information (see class Joint_Info) + self.joints_transform = [Matrix_4x4() for _ in range(self.no_of_joints)] # Joints' transformation matrix + + # Localization variables relative to head + self.loc_head_to_field_transform = Matrix_4x4() # Transformation matrix from head to field + self.loc_field_to_head_transform = Matrix_4x4() # Transformation matrix from field to head + self.loc_rotation_head_to_field = Matrix_3x3() # Rotation matrix from head to field + self.loc_rotation_field_to_head = Matrix_3x3() # Rotation matrix from field to head + self.loc_head_position = np.zeros(3) # Absolute head position (m) + self.loc_head_position_history = deque(maxlen=40)# Absolute head position history (queue with up to 40 old positions at intervals of 0.04s, where index 0 is the previous position) + self.loc_head_velocity = np.zeros(3) # Absolute head velocity (m/s) (Warning: possibly noisy) + self.loc_head_orientation = 0 # Head orientation (deg) + self.loc_is_up_to_date = False # False if this is not a visual step, or not enough elements are visible + self.loc_last_update = 0 # World.time_local_ms when the localization was last updated + self.loc_head_position_last_update = 0 # World.time_local_ms when loc_head_position was last updated by vision or radio + self.radio_fallen_state = False # True if (radio says we fell) and (radio is significantly more recent than loc) + self.radio_last_update = 0 # World.time_local_ms when radio_fallen_state was last updated (and possibly loc_head_position) + + # Localization variables relative to torso + self.loc_torso_to_field_rotation = Matrix_3x3() # Rotation matrix from torso to field + self.loc_torso_to_field_transform = Matrix_4x4() # Transformation matrix from torso to field + self.loc_torso_roll = 0 # Torso roll (deg) + self.loc_torso_pitch = 0 # Torso pitch (deg) + self.loc_torso_orientation = 0 # Torso orientation (deg) + self.loc_torso_inclination = 0 # Torso inclination (deg) (inclination of z-axis in relation to field z-axis) + self.loc_torso_position = np.zeros(3) # Absolute torso position (m) + self.loc_torso_velocity = np.zeros(3) # Absolute torso velocity (m/s) + self.loc_torso_acceleration = np.zeros(3) # Absolute Coordinate acceleration (m/s2) + + # Other localization variables + self.cheat_abs_pos = np.zeros(3) # Absolute head position provided by the server as cheat (m) + self.cheat_ori = 0.0 # Absolute head orientation provided by the server as cheat (deg) + self.loc_CoM_position = np.zeros(3) # Absolute CoM position (m) + self.loc_CoM_velocity = np.zeros(3) # Absolute CoM velocity (m/s) + + # Localization special variables + ''' + self.loc_head_z is often equivalent to self.loc_head_position[2], but sometimes it differs. + There are situations in which the rotation and translation cannot be computed, + but the z-coordinate can still be found through vision, in which case: + self.loc_is_up_to_date is False + self.loc_head_z_is_up_to_date is True + It should be used in applications which rely on z as an independent coordinate, such + as detecting if the robot has fallen, or as an observation for machine learning. + It should NEVER be used for 3D transformations. + ''' + self.loc_head_z = 0 # Absolute head position (z) - see above for explanation (m) + self.loc_head_z_is_up_to_date = False # False if this is not a visual step, or not enough elements are visible + self.loc_head_z_last_update = 0 # World.time_local_ms when loc_head_z was last computed + self.loc_head_z_vel = 0 # Absolute head velocity (z) (m/s) + + # Localization + Gyroscope + # These variables are reliable. The gyroscope is used to update the rotation when waiting for the next visual cycle + self.imu_torso_roll = 0 # Torso roll (deg) (src: Localization + Gyro) + self.imu_torso_pitch = 0 # Torso pitch (deg) (src: Localization + Gyro) + self.imu_torso_orientation = 0 # Torso orientation (deg) (src: Localization + Gyro) + self.imu_torso_inclination = 0 # Torso inclination (deg) (src: Localization + Gyro) + self.imu_torso_to_field_rotation = Matrix_3x3() # Rotation matrix from torso to field (src: Localization + Gyro) + self.imu_last_visual_update = 0 # World.time_local_ms when the IMU data was last updated with visual information + + # Localization + Gyroscope + Accelerometer + # Warning: these variables are unreliable, since small errors in the Localization Orientation lead to + # wrong acceleration -> wrong velocity -> wrong position + self.imu_weak_torso_to_field_transform = Matrix_4x4() # Transformation matrix from torso to field (src: Localization + Gyro + Acc) + self.imu_weak_head_to_field_transform = Matrix_4x4() # Transformation matrix from head to field (src: Localization + Gyro + Acc) + self.imu_weak_field_to_head_transform = Matrix_4x4() # Transformation matrix from field to head (src: Localization + Gyro + Acc) + self.imu_weak_torso_position = np.zeros(3) # Absolute torso position (m) (src: Localization + Gyro + Acc) + self.imu_weak_torso_velocity = np.zeros(3) # Absolute torso velocity (m/s) (src: Localization + Gyro + Acc) + self.imu_weak_torso_acceleration = np.zeros(3) # Absolute torso acceleration (m/s2) (src: Localization + Gyro + Acc) + self.imu_weak_torso_next_position = np.zeros(3) # Absolute position in next step estimate (m) (src: Localization + Gyro + Acc) + self.imu_weak_torso_next_velocity = np.zeros(3) # Absolute velocity in next step estimate (m/s) (src: Localization + Gyro + Acc) + self.imu_weak_CoM_position = np.zeros(3) # Absolute CoM position (m) (src: Localization + Gyro + Acc) + self.imu_weak_CoM_velocity = np.zeros(3) # Absolute CoM velocity (m/s) (src: Localization + Gyro + Acc) + + + #Using explicit variables to enable IDE suggestions + self.J_HEAD_YAW = 0 + self.J_HEAD_PITCH = 1 + self.J_LLEG_YAW_PITCH = 2 + self.J_RLEG_YAW_PITCH = 3 + self.J_LLEG_ROLL = 4 + self.J_RLEG_ROLL = 5 + self.J_LLEG_PITCH = 6 + self.J_RLEG_PITCH = 7 + self.J_LKNEE = 8 + self.J_RKNEE = 9 + self.J_LFOOT_PITCH = 10 + self.J_RFOOT_PITCH = 11 + self.J_LFOOT_ROLL = 12 + self.J_RFOOT_ROLL = 13 + self.J_LARM_PITCH = 14 + self.J_RARM_PITCH = 15 + self.J_LARM_ROLL = 16 + self.J_RARM_ROLL = 17 + self.J_LELBOW_YAW = 18 + self.J_RELBOW_YAW = 19 + self.J_LELBOW_ROLL = 20 + self.J_RELBOW_ROLL = 21 + self.J_LTOE_PITCH = 22 + self.J_RTOE_PITCH = 23 + + + #------------------ parse robot xml + + dir = M.get_active_directory("/world/commons/robots/") + robot_xml_root = xmlp.parse(dir + robot_xml).getroot() + + joint_no = 0 + for child in robot_xml_root: + if child.tag == "bodypart": + self.body_parts[child.attrib['name']] = Body_Part(child.attrib['mass']) + elif child.tag == "joint": + self.joints_info[joint_no] = Joint_Info(child) + self.joints_position[joint_no] = 0.0 + ji = self.joints_info[joint_no] + + #save joint if body part is 1st anchor (to simplify model traversal in a single direction) + self.body_parts[ji.anchor0_part].joints.append(Robot.MAP_PERCEPTOR_TO_INDEX[ji.perceptor]) + + joint_no += 1 + if joint_no == self.no_of_joints: break #ignore extra joints + + else: + raise NotImplementedError + + assert joint_no == self.no_of_joints, "The Robot XML and the robot type don't match!" + + + def get_head_abs_vel(self, history_steps:int): + ''' + Get robot's head absolute velocity (m/s) + + Parameters + ---------- + history_steps : int + number of history steps to consider [1,40] + + Examples + -------- + get_head_abs_vel(1) is equivalent to (current abs pos - last abs pos) / 0.04 + get_head_abs_vel(2) is equivalent to (current abs pos - abs pos 0.08s ago) / 0.08 + get_head_abs_vel(3) is equivalent to (current abs pos - abs pos 0.12s ago) / 0.12 + ''' + assert 1 <= history_steps <= 40, "Argument 'history_steps' must be in range [1,40]" + + if len(self.loc_head_position_history) == 0: + return np.zeros(3) + + h_step = min(history_steps, len(self.loc_head_position_history)) + t = h_step * Robot.VISUALSTEP + + return (self.loc_head_position - self.loc_head_position_history[h_step-1]) / t + + + def _initialize_kinematics(self): + + #starting with head + parts={"head"} + sequential_body_parts = ["head"] + + while len(parts) > 0: + part = parts.pop() + + for j in self.body_parts[part].joints: + + p = self.joints_info[j].anchor1_part + + if len(self.body_parts[p].joints) > 0: #add body part if it is the 1st anchor of some joint + parts.add(p) + sequential_body_parts.append(p) + + self.fwd_kinematics_list = [(self.body_parts[part],j, self.body_parts[self.joints_info[j].anchor1_part] ) + for part in sequential_body_parts for j in self.body_parts[part].joints] + + #Fix symmetry issues 4/4 (kinematics) + for i in Robot.FIX_INDICES_LIST: + self.joints_info[i].axes *= -1 + aux = self.joints_info[i].min + self.joints_info[i].min = -self.joints_info[i].max + self.joints_info[i].max = -aux + + + def update_localization(self, localization_raw, time_local_ms): + + # parse raw data + loc = localization_raw.astype(float) #32bits to 64bits for consistency + self.loc_is_up_to_date = bool(loc[32]) + self.loc_head_z_is_up_to_date = bool(loc[34]) + + if self.loc_head_z_is_up_to_date: + time_diff = (time_local_ms - self.loc_head_z_last_update) / 1000 + self.loc_head_z_vel = (loc[33] - self.loc_head_z) / time_diff + self.loc_head_z = loc[33] + self.loc_head_z_last_update = time_local_ms + + # Save last position to history at every vision cycle (even if not up to date) (update_localization is only called at vision cycles) + self.loc_head_position_history.appendleft(np.copy(self.loc_head_position)) + + if self.loc_is_up_to_date: + time_diff = (time_local_ms - self.loc_last_update) / 1000 + self.loc_last_update = time_local_ms + self.loc_head_to_field_transform.m[:] = loc[0:16].reshape((4,4)) + self.loc_field_to_head_transform.m[:] = loc[16:32].reshape((4,4)) + + # extract data (related to the robot's head) + self.loc_rotation_head_to_field = self.loc_head_to_field_transform.get_rotation() + self.loc_rotation_field_to_head = self.loc_field_to_head_transform.get_rotation() + p = self.loc_head_to_field_transform.get_translation() + self.loc_head_velocity = (p - self.loc_head_position) / time_diff + self.loc_head_position = p + self.loc_head_position_last_update = time_local_ms + self.loc_head_orientation = self.loc_head_to_field_transform.get_yaw_deg() + self.radio_fallen_state = False + + # extract data (related to the center of mass) + p = self.loc_head_to_field_transform(self.rel_cart_CoM_position) + self.loc_CoM_velocity = (p - self.loc_CoM_position) / time_diff + self.loc_CoM_position = p + + # extract data (related to the robot's torso) + t = self.get_body_part_to_field_transform('torso') + self.loc_torso_to_field_transform = t + self.loc_torso_to_field_rotation = t.get_rotation() + self.loc_torso_orientation = t.get_yaw_deg() + self.loc_torso_pitch = t.get_pitch_deg() + self.loc_torso_roll = t.get_roll_deg() + self.loc_torso_inclination = t.get_inclination_deg() + p = t.get_translation() + self.loc_torso_velocity = (p - self.loc_torso_position) / time_diff + self.loc_torso_position = p + self.loc_torso_acceleration = self.loc_torso_to_field_rotation.multiply(self.acc) + Robot.GRAVITY + + + def head_to_body_part_transform(self, body_part_name, coords, is_batch=False): + ''' + If coord is a vector or list of vectors: + Convert cartesian coordinates that are relative to head to coordinates that are relative to a body part + + If coord is a Matrix_4x4 or a list of Matrix_4x4: + Convert pose that is relative to head to a pose that is relative to a body part + + Parameters + ---------- + body_part_name : `str` + name of body part (given by the robot's XML) + coords : array_like + One 3D position or list of 3D positions + is_batch : `bool` + Indicates if coords is a batch of 3D positions + + Returns + ------- + coord : `list` or ndarray + A numpy array is returned if is_batch is False, otherwise, a list of arrays is returned + ''' + head_to_bp_transform : Matrix_4x4 = self.body_parts[body_part_name].transform.invert() + + if is_batch: + return [head_to_bp_transform(c) for c in coords] + else: + return head_to_bp_transform(coords) + + + + def get_body_part_to_field_transform(self, body_part_name) -> Matrix_4x4: + ''' + Computes the transformation matrix from body part to field, from which we can extract its absolute position and rotation. + For best results, use this method when self.loc_is_up_to_date is True. Otherwise, the forward kinematics + will not be synced with the localization data and strange results may occur. + ''' + return self.loc_head_to_field_transform.multiply(self.body_parts[body_part_name].transform) + + def get_body_part_abs_position(self, body_part_name) -> np.ndarray: + ''' + Computes the absolute position of a body part considering the localization data and forward kinematics. + For best results, use this method when self.loc_is_up_to_date is True. Otherwise, the forward kinematics + will not be synced with the localization data and strange results may occur. + ''' + return self.get_body_part_to_field_transform(body_part_name).get_translation() + + def get_joint_to_field_transform(self, joint_index) -> Matrix_4x4: + ''' + Computes the transformation matrix from joint to field, from which we can extract its absolute position and rotation. + For best results, use this method when self.loc_is_up_to_date is True. Otherwise, the forward kinematics + will not be synced with the localization data and strange results may occur. + ''' + return self.loc_head_to_field_transform.multiply(self.joints_transform[joint_index]) + + def get_joint_abs_position(self, joint_index) -> np.ndarray: + ''' + Computes the absolute position of a joint considering the localization data and forward kinematics. + For best results, use this method when self.loc_is_up_to_date is True. Otherwise, the forward kinematics + will not be synced with the localization data and strange results may occur. + ''' + return self.get_joint_to_field_transform(joint_index).get_translation() + + def update_pose(self): + + if self.fwd_kinematics_list is None: + self._initialize_kinematics() + + for body_part, j, child_body_part in self.fwd_kinematics_list: + ji = self.joints_info[j] + self.joints_transform[j].m[:] = body_part.transform.m + self.joints_transform[j].translate(ji.anchor0_axes, True) + child_body_part.transform.m[:] = self.joints_transform[j].m + child_body_part.transform.rotate_deg(ji.axes, self.joints_position[j], True) + child_body_part.transform.translate(ji.anchor1_axes_neg, True) + + self.rel_cart_CoM_position = np.average([b.transform.get_translation() for b in self.body_parts.values()], 0, + [b.mass for b in self.body_parts.values()]) + + + def update_imu(self, time_local_ms): + + # update IMU + if self.loc_is_up_to_date: + self.imu_torso_roll = self.loc_torso_roll + self.imu_torso_pitch = self.loc_torso_pitch + self.imu_torso_orientation = self.loc_torso_orientation + self.imu_torso_inclination = self.loc_torso_inclination + self.imu_torso_to_field_rotation.m[:] = self.loc_torso_to_field_rotation.m + self.imu_weak_torso_to_field_transform.m[:] = self.loc_torso_to_field_transform.m + self.imu_weak_head_to_field_transform.m[:] = self.loc_head_to_field_transform.m + self.imu_weak_field_to_head_transform.m[:] = self.loc_field_to_head_transform.m + self.imu_weak_torso_position[:] = self.loc_torso_position + self.imu_weak_torso_velocity[:] = self.loc_torso_velocity + self.imu_weak_torso_acceleration[:] = self.loc_torso_acceleration + self.imu_weak_torso_next_position = self.loc_torso_position + self.loc_torso_velocity * Robot.STEPTIME + self.loc_torso_acceleration * (0.5 * Robot.SQ_STEPTIME) + self.imu_weak_torso_next_velocity = self.loc_torso_velocity + self.loc_torso_acceleration * Robot.STEPTIME + self.imu_weak_CoM_position[:] = self.loc_CoM_position + self.imu_weak_CoM_velocity[:] = self.loc_CoM_velocity + self.imu_last_visual_update = time_local_ms + else: + g = self.gyro / 50 # convert degrees per second to degrees per step + + self.imu_torso_to_field_rotation.multiply( Matrix_3x3.from_rotation_deg(g), in_place=True, reverse_order=True) + + self.imu_torso_orientation = self.imu_torso_to_field_rotation.get_yaw_deg() + self.imu_torso_pitch = self.imu_torso_to_field_rotation.get_pitch_deg() + self.imu_torso_roll = self.imu_torso_to_field_rotation.get_roll_deg() + + self.imu_torso_inclination = atan(sqrt(tan(self.imu_torso_roll/180*pi)**2+tan(self.imu_torso_pitch/180*pi)**2))*180/pi + + # Update position and velocity until 0.2 seconds has passed since last visual update + if time_local_ms < self.imu_last_visual_update + 200: + self.imu_weak_torso_position[:] = self.imu_weak_torso_next_position + if self.imu_weak_torso_position[2] < 0: self.imu_weak_torso_position[2] = 0 # limit z coordinate to positive values + self.imu_weak_torso_velocity[:] = self.imu_weak_torso_next_velocity * Robot.IMU_DECAY # stability tradeoff + else: + self.imu_weak_torso_velocity *= 0.97 # without visual updates for 0.2s, the position is locked, and the velocity decays to zero + + # convert proper acceleration to coordinate acceleration and fix rounding bias + self.imu_weak_torso_acceleration = self.imu_torso_to_field_rotation.multiply(self.acc) + Robot.GRAVITY + self.imu_weak_torso_to_field_transform = Matrix_4x4.from_3x3_and_translation(self.imu_torso_to_field_rotation,self.imu_weak_torso_position) + self.imu_weak_head_to_field_transform = self.imu_weak_torso_to_field_transform.multiply(self.body_parts["torso"].transform.invert()) + self.imu_weak_field_to_head_transform = self.imu_weak_head_to_field_transform.invert() + p = self.imu_weak_head_to_field_transform(self.rel_cart_CoM_position) + self.imu_weak_CoM_velocity = (p-self.imu_weak_CoM_position)/Robot.STEPTIME + self.imu_weak_CoM_position = p + + # Next Position = x0 + v0*t + 0.5*a*t^2, Next velocity = v0 + a*t + self.imu_weak_torso_next_position = self.imu_weak_torso_position + self.imu_weak_torso_velocity * Robot.STEPTIME + self.imu_weak_torso_acceleration * (0.5 * Robot.SQ_STEPTIME) + self.imu_weak_torso_next_velocity = self.imu_weak_torso_velocity + self.imu_weak_torso_acceleration * Robot.STEPTIME + + + + def set_joints_target_position_direct(self,indices,values:np.ndarray,harmonize=True,max_speed=7.03,tolerance=0.012,limit_joints=True) -> int: + ''' + Computes the speed of a list of joints, taking as argument the target position + + Parameters + ---------- + indices : `int`/`list`/`slice`/numpy array + joint indices + values : numpy array + target position for each listed joint index + harmonize : `bool` + if True, all joints reach target at same time + max_speed : `float` + max. speed for all joints in deg/step + Most joints have a maximum speed of 351.77 deg/s according to rcssserver3d/data/rsg/agent/nao/hingejoint.rsg + That translates as 7.0354 deg/step or 6.1395 rad/s + tolerance : `float` + angle error tolerance (in degrees) to return that target was reached (returns -1) + limit_joints : `bool` + limit values to the joints' range of motion + + Returns + ------- + remaining_steps : `int` + predicted number of remaining steps or -1 if target was already reached + + Examples + ------- + (let p[tx] be the joint position at t=x) + + Example for return value: moving joint[0] from 0deg to 10deg + pos[t0]: 0, speed[t0]: 7deg/step, ret=2 # target will predictedly be reached in 2 steps + pos[t1]: 7, speed[t1]: 3deg/step, ret=1 # target will predictedly be reached in 1 step (send final action) + pos[t2]: 10, speed[t2]: 0deg/step, ret=0 # target was predictedly already reached + pos[t3]: 10, speed[t3]: 0deg/step, ret=-1 # (best case scenario) server reported with delay, that target was reached (see tolerance) + pos[t?]: 10, speed[t?]: 0deg/step, ret=-1 # if there is friction, it may take some additional steps + + If everything worked as predicted we could stop calling this function when ret==1 + If we need precision, it is recommended to wait for ret==-1 + + Example 1: + set_joints_target_position_direct(range(2,4),np.array([10.0,5.0]),harmonize=True) + Joint[2] p[t0]: 0 target pos: 10 -> p[t1]=5, p[t2]=10 + Joint[3] p[t0]: 0 target pos: 5 -> p[t1]=2.5, p[t2]=5 + + Example 2: + set_joints_target_position_direct([2,3],np.array([10.0,5.0]),harmonize=False) + Joint[2] p[t0]: 0 target pos: 10 -> p[t1]=7, p[t2]=10 + Joint[3] p[t0]: 0 target pos: 5 -> p[t1]=5, p[t2]=5 + ''' + + assert type(values) == np.ndarray, "'values' argument must be a numpy array" + np.nan_to_num(values, copy=False) # Replace NaN with zero and infinity with large finite numbers + + # limit range of joints + if limit_joints: + if type(indices) == list or type(indices) == np.ndarray: + for i in range(len(indices)): + values[i] = np.clip(values[i], self.joints_info[indices[i]].min, self.joints_info[indices[i]].max) + elif type(indices) == slice: + info = self.joints_info[indices] + for i in range(len(info)): + values[i] = np.clip(values[i], info[i].min, info[i].max) + else: # int + values[0] = np.clip(values[0], self.joints_info[indices].min, self.joints_info[indices].max) + + #predicted_diff: predicted difference between reported position and actual position + + predicted_diff = self.joints_target_last_speed[indices] * 1.1459156 #rad/s to deg/step + predicted_diff = np.asarray(predicted_diff) + np.clip(predicted_diff,-7.03,7.03,out=predicted_diff) #saturate predicted movement in-place + + #reported_dist: difference between reported position and target position + + reported_dist = values - self.joints_position[indices] + if np.all((np.abs(reported_dist) < tolerance)) and np.all((np.abs(predicted_diff) < tolerance)): + self.joints_target_speed[indices] = 0 + return -1 + + deg_per_step = reported_dist - predicted_diff + + relative_max = np.max( np.abs(deg_per_step) ) / max_speed + remaining_steps = np.ceil( relative_max ) + + if remaining_steps == 0: + self.joints_target_speed[indices] = 0 + return 0 + + if harmonize: + deg_per_step /= remaining_steps + else: + np.clip(deg_per_step,-max_speed,max_speed,out=deg_per_step) #limit maximum speed + + self.joints_target_speed[indices] = deg_per_step * 0.87266463 #convert to rad/s + + return remaining_steps + + + + def get_command(self) -> bytes: + ''' + Builds commands string from self.joints_target_speed + ''' + j_speed = self.joints_target_speed * self.FIX_EFFECTOR_MASK #Fix symmetry issues 3/4 (effectors) + cmd = "".join(f"({self.joints_info[i].effector} {j_speed[i]:.5f})" for i in range(self.no_of_joints)).encode('utf-8') + + self.joints_target_last_speed = self.joints_target_speed #1. both point to the same array + self.joints_target_speed = np.zeros_like(self.joints_target_speed) #2. create new array for joints_target_speed + return cmd diff --git a/world/World.py b/world/World.py new file mode 100644 index 0000000..9c1f549 --- /dev/null +++ b/world/World.py @@ -0,0 +1,430 @@ +from collections import deque +from cpp.ball_predictor import ball_predictor +from cpp.localization import localization +from logs.Logger import Logger +from math import atan2, pi +from math_ops.Matrix_4x4 import Matrix_4x4 +from world.commons.Draw import Draw +from world.commons.Other_Robot import Other_Robot +from world.Robot import Robot +import numpy as np + + +class World(): + STEPTIME = 0.02 # Fixed step time + STEPTIME_MS = 20 # Fixed step time in milliseconds + VISUALSTEP = 0.04 # Fixed visual step time + VISUALSTEP_MS = 40 # Fixed visual step time in milliseconds + + # play modes in our favor + M_OUR_KICKOFF = 0 + M_OUR_KICK_IN = 1 + M_OUR_CORNER_KICK = 2 + M_OUR_GOAL_KICK = 3 + M_OUR_FREE_KICK = 4 + M_OUR_PASS = 5 + M_OUR_DIR_FREE_KICK = 6 + M_OUR_GOAL = 7 + M_OUR_OFFSIDE = 8 + + # play modes in their favor + M_THEIR_KICKOFF = 9 + M_THEIR_KICK_IN = 10 + M_THEIR_CORNER_KICK = 11 + M_THEIR_GOAL_KICK = 12 + M_THEIR_FREE_KICK = 13 + M_THEIR_PASS = 14 + M_THEIR_DIR_FREE_KICK = 15 + M_THEIR_GOAL = 16 + M_THEIR_OFFSIDE = 17 + + # neutral play modes + M_BEFORE_KICKOFF = 18 + M_GAME_OVER = 19 + M_PLAY_ON = 20 + + # play mode groups + MG_OUR_KICK = 0 + MG_THEIR_KICK = 1 + MG_ACTIVE_BEAM = 2 + MG_PASSIVE_BEAM = 3 + MG_OTHER = 4 # play on, game over + + FLAGS_CORNERS_POS = ((-15,-10,0), (-15,+10,0), (+15,-10,0), (+15,+10,0)) + FLAGS_POSTS_POS = ((-15,-1.05,0.8),(-15,+1.05,0.8),(+15,-1.05,0.8),(+15,+1.05,0.8)) + + def __init__(self,robot_type:int, team_name:str, unum:int, apply_play_mode_correction:bool, + enable_draw:bool, logger:Logger, host:str) -> None: + + self.team_name = team_name # Name of our team + self.team_name_opponent : str = None # Name of opponent team + self.apply_play_mode_correction = apply_play_mode_correction # True to adjust ball position according to play mode + self.step = 0 # Total number of received simulation steps (always in sync with self.time_local_ms) + self.time_server = 0.0 # Time, in seconds, as indicated by the server (this time is NOT reliable, use only for synchronization between agents) + self.time_local_ms = 0 # Reliable simulation time in milliseconds, use this when possible (it is incremented 20ms for every TCP message) + self.time_game = 0.0 # Game time, in seconds, as indicated by the server + self.goals_scored = 0 # Goals score by our team + self.goals_conceded = 0 # Goals conceded by our team + self.team_side_is_left : bool = None # True if our team plays on the left side (this value is later changed by the world parser) + self.play_mode = None # Play mode of the soccer game, provided by the server + self.play_mode_group = None # Certain play modes share characteristics, so it makes sense to group them + self.flags_corners : dict = None # corner flags, key=(x,y,z), always assume we play on the left side + self.flags_posts : dict = None # goal posts, key=(x,y,z), always assume we play on the left side + self.ball_rel_head_sph_pos = np.zeros(3) # Ball position relative to head (spherical coordinates) (m, deg, deg) + self.ball_rel_head_cart_pos = np.zeros(3) # Ball position relative to head (cartesian coordinates) (m) + self.ball_rel_torso_cart_pos = np.zeros(3) # Ball position relative to torso (cartesian coordinates) (m) + self.ball_rel_torso_cart_pos_history = deque(maxlen=20) # Ball position relative to torso history (queue with up to 20 old positions at intervals of 0.04s, where index 0 is the previous position) + self.ball_abs_pos = np.zeros(3) # Ball absolute position (up to date if self.ball_is_visible and self.robot.loc_is_up_to_date) (m) + self.ball_abs_pos_history = deque(maxlen=20) # Ball absolute position history (queue with up to 20 old positions at intervals of 0.04s, where index 0 is the previous position) + self.ball_abs_pos_last_update = 0 # World.time_local_ms when self.ball_abs_pos was last updated by vision or radio + self.ball_abs_vel = np.zeros(3) # Ball velocity vector based on the last 2 known values of self.ball_abs_pos (m/s) (Warning: noisy if ball is distant, use instead get_ball_abs_vel) + self.ball_abs_speed = 0 # Ball scalar speed based on the last 2 known values of self.ball_abs_pos (m/s) (Warning: noisy if ball is distant, use instead ||get_ball_abs_vel||) + self.ball_is_visible = False # True if the last server message contained vision information related to the ball + self.is_ball_abs_pos_from_vision = False # True if ball_abs_pos originated from vision, False if it originated from radio + self.ball_last_seen = 0 # World.time_local_ms when ball was last seen (note: may be different from self.ball_abs_pos_last_update) + self.ball_cheat_abs_pos = np.zeros(3) # Absolute ball position provided by the server as cheat (m) + self.ball_cheat_abs_vel = np.zeros(3) # Absolute velocity vector based on the last 2 values of self.ball_cheat_abs_pos (m/s) + self.ball_2d_pred_pos = np.zeros((1,2)) # prediction of current and future 2D ball positions* + self.ball_2d_pred_vel = np.zeros((1,2)) # prediction of current and future 2D ball velocities* + self.ball_2d_pred_spd = np.zeros(1) # prediction of current and future 2D ball linear speeds* + # *at intervals of 0.02 s until ball comes to a stop or gets out of bounds (according to prediction) + self.lines = np.zeros((30,6)) # Position of visible lines, relative to head, start_pos+end_pos (spherical coordinates) (m, deg, deg, m, deg, deg) + self.line_count = 0 # Number of visible lines + self.vision_last_update = 0 # World.time_local_ms when last vision update was received + self.vision_is_up_to_date = False # True if the last server message contained vision information + self.teammates = [Other_Robot(i, True ) for i in range(1,12)] # List of teammates, ordered by unum + self.opponents = [Other_Robot(i, False) for i in range(1,12)] # List of opponents, ordered by unum + self.teammates[unum-1].is_self = True # This teammate is self + self.draw = Draw(enable_draw, unum, host, 32769) # Draw object for current player + self.team_draw = Draw(enable_draw, 0, host, 32769) # Draw object shared with teammates + self.logger = logger + self.robot = Robot(unum, robot_type) + + + def log(self, msg:str): + ''' + Shortcut for: + + self.logger.write(msg, True, self.step) + + Parameters + ---------- + msg : str + message to be written after the simulation step + ''' + self.logger.write(msg, True, self.step) + + def get_ball_rel_vel(self, history_steps:int): + ''' + Get ball velocity, relative to torso (m/s) + + Parameters + ---------- + history_steps : int + number of history steps to consider [1,20] + + Examples + -------- + get_ball_rel_vel(1) is equivalent to (current rel pos - last rel pos) / 0.04 + get_ball_rel_vel(2) is equivalent to (current rel pos - rel pos 0.08s ago) / 0.08 + get_ball_rel_vel(3) is equivalent to (current rel pos - rel pos 0.12s ago) / 0.12 + ''' + assert 1 <= history_steps <= 20, "Argument 'history_steps' must be in range [1,20]" + + if len(self.ball_rel_torso_cart_pos_history) == 0: + return np.zeros(3) + + h_step = min(history_steps, len(self.ball_rel_torso_cart_pos_history)) + t = h_step * World.VISUALSTEP + + return (self.ball_rel_torso_cart_pos - self.ball_rel_torso_cart_pos_history[h_step-1]) / t + + def get_ball_abs_vel(self, history_steps:int): + ''' + Get ball absolute velocity (m/s) + + Parameters + ---------- + history_steps : int + number of history steps to consider [1,20] + + Examples + -------- + get_ball_abs_vel(1) is equivalent to (current abs pos - last abs pos) / 0.04 + get_ball_abs_vel(2) is equivalent to (current abs pos - abs pos 0.08s ago) / 0.08 + get_ball_abs_vel(3) is equivalent to (current abs pos - abs pos 0.12s ago) / 0.12 + ''' + assert 1 <= history_steps <= 20, "Argument 'history_steps' must be in range [1,20]" + + if len(self.ball_abs_pos_history) == 0: + return np.zeros(3) + + h_step = min(history_steps, len(self.ball_abs_pos_history)) + t = h_step * World.VISUALSTEP + + return (self.ball_abs_pos - self.ball_abs_pos_history[h_step-1]) / t + + def get_predicted_ball_pos(self, max_speed): + ''' + Get predicted 2D ball position when its predicted speed is equal to or less than `max_speed` + In case that position exceeds the prediction horizon, the last available prediction is returned + + Parameters + ---------- + max_speed : float + maximum speed at which the ball will be moving at returned future position + ''' + b_sp = self.ball_2d_pred_spd + index = len(b_sp) - max( 1, np.searchsorted(b_sp[::-1], max_speed, side='right') ) + return self.ball_2d_pred_pos[index] + + def get_intersection_point_with_ball(self, player_speed): + ''' + Get 2D intersection point with moving ball, based on `self.ball_2d_pred_pos` + + Parameters + ---------- + player_speed : float + average speed at which the robot will chase the ball + + Returns + ------- + 2D intersection point : ndarray + 2D intersection point with moving ball, assuming the robot moves at an avg. speed of `player_speed` + intersection distance : float + distance between current robot position and intersection point + ''' + + params = np.array([*self.robot.loc_head_position[:2], player_speed*0.02, *self.ball_2d_pred_pos.flat], np.float32) + pred_ret = ball_predictor.get_intersection(params) + return pred_ret[:2], pred_ret[2] + + def update(self): + r = self.robot + PM = self.play_mode + W = World + + # reset variables + r.loc_is_up_to_date = False + r.loc_head_z_is_up_to_date = False + + # update play mode groups + if PM in (W.M_PLAY_ON, W.M_GAME_OVER): # most common group + self.play_mode_group = W.MG_OTHER + elif PM in (W.M_OUR_KICKOFF, W.M_OUR_KICK_IN, W.M_OUR_CORNER_KICK, W.M_OUR_GOAL_KICK, + W.M_OUR_OFFSIDE, W.M_OUR_PASS, W.M_OUR_DIR_FREE_KICK, W.M_OUR_FREE_KICK): + self.play_mode_group = W.MG_OUR_KICK + elif PM in (W.M_THEIR_KICK_IN, W.M_THEIR_CORNER_KICK, W.M_THEIR_GOAL_KICK, W.M_THEIR_OFFSIDE, + W.M_THEIR_PASS, W.M_THEIR_DIR_FREE_KICK, W.M_THEIR_FREE_KICK, W.M_THEIR_KICKOFF): + self.play_mode_group = W.MG_THEIR_KICK + elif PM in (W.M_BEFORE_KICKOFF, W.M_THEIR_GOAL): + self.play_mode_group = W.MG_ACTIVE_BEAM + elif PM in (W.M_OUR_GOAL,): + self.play_mode_group = W.MG_PASSIVE_BEAM + elif PM is not None: + raise ValueError(f'Unexpected play mode ID: {PM}') + + r.update_pose() # update forward kinematics + if self.vision_is_up_to_date: # update vision based localization + + # Prepare all variables for localization + + feet_contact = np.zeros(6) + + lf_contact = r.frp.get('lf', None) + rf_contact = r.frp.get('rf', None) + if lf_contact is not None: + feet_contact[0:3] = Matrix_4x4( r.body_parts["lfoot"].transform ).translate( lf_contact[0:3] , True).get_translation() + if rf_contact is not None: + feet_contact[3:6] = Matrix_4x4( r.body_parts["rfoot"].transform ).translate( rf_contact[0:3] , True).get_translation() + + ball_pos = np.concatenate(( self.ball_rel_head_cart_pos, self.ball_cheat_abs_pos)) + + corners_list = [[key in self.flags_corners, 1.0, *key, *self.flags_corners.get(key,(0,0,0))] for key in World.FLAGS_CORNERS_POS] + posts_list = [[key in self.flags_posts , 0.0, *key, *self.flags_posts.get( key,(0,0,0))] for key in World.FLAGS_POSTS_POS] + all_landmarks = np.array(corners_list + posts_list, float) + + # Compute localization + + loc = localization.compute( + r.feet_toes_are_touching['lf'], + r.feet_toes_are_touching['rf'], + feet_contact, + self.ball_is_visible, + ball_pos, + r.cheat_abs_pos, + all_landmarks, + self.lines[0:self.line_count]) + + r.update_localization(loc, self.time_local_ms) + + # Update self in teammates list (only the most useful parameters, add as needed) + me = self.teammates[r.unum-1] + me.state_last_update = r.loc_last_update + me.state_abs_pos = r.loc_head_position + me.state_fallen = r.loc_head_z < 0.3 # uses same criterion as for other teammates - not as reliable as player.behavior.is_ready("Get_Up") + me.state_orientation = r.loc_torso_orientation + me.state_ground_area = (r.loc_head_position[:2],0.2) # relevant for localization demo + + # Save last ball position to history at every vision cycle (even if not up to date) + self.ball_abs_pos_history.appendleft(self.ball_abs_pos) # from vision or radio + self.ball_rel_torso_cart_pos_history.appendleft(self.ball_rel_head_cart_pos) + + ''' + Get ball position based on vision or play mode + Sources: + Corner kick position - rcssserver3d/plugin/soccer/soccerruleaspect/soccerruleaspect.cpp:1927 (May 2022) + Goal kick position - rcssserver3d/plugin/soccer/soccerruleaspect/soccerruleaspect.cpp:1900 (May 2022) + ''' + ball = None + if self.apply_play_mode_correction: + if PM == W.M_OUR_CORNER_KICK: + ball = np.array([15, 5.483 if self.ball_abs_pos[1] > 0 else -5.483, 0.042], float) + elif PM == W.M_THEIR_CORNER_KICK: + ball = np.array([-15, 5.483 if self.ball_abs_pos[1] > 0 else -5.483, 0.042], float) + elif PM in [W.M_OUR_KICKOFF, W.M_THEIR_KICKOFF, W.M_OUR_GOAL, W.M_THEIR_GOAL]: + ball = np.array([0, 0, 0.042], float) + elif PM == W.M_OUR_GOAL_KICK: + ball = np.array([-14, 0, 0.042], float) + elif PM == W.M_THEIR_GOAL_KICK: + ball = np.array([14, 0, 0.042], float) + + # Discard hard-coded ball position if robot is near that position (in favor of its own vision) + if ball is not None and np.linalg.norm(r.loc_head_position[:2] - ball[:2]) < 1: + ball = None + + if ball is None and self.ball_is_visible and r.loc_is_up_to_date: + ball = r.loc_head_to_field_transform( self.ball_rel_head_cart_pos ) + ball[2] = max(ball[2], 0.042) # lowest z = ball radius + if PM != W.M_BEFORE_KICKOFF: # for compatibility with tests without active soccer rules + ball[:2] = np.clip(ball[:2], [-15,-10], [15,10]) # force ball position to be inside field + + # Update internal ball position (also updated by Radio) + if ball is not None: + time_diff = (self.time_local_ms - self.ball_abs_pos_last_update) / 1000 + self.ball_abs_vel = (ball - self.ball_abs_pos) / time_diff + self.ball_abs_speed = np.linalg.norm(self.ball_abs_vel) + self.ball_abs_pos_last_update = self.time_local_ms + self.ball_abs_pos = ball + self.is_ball_abs_pos_from_vision = True + + # Velocity decay for teammates and opponents (it is later neutralized if the velocity is updated) + for p in self.teammates: + p.state_filtered_velocity *= p.vel_decay + for p in self.opponents: + p.state_filtered_velocity *= p.vel_decay + + # Update teammates and opponents + if r.loc_is_up_to_date: + for p in self.teammates: + if not p.is_self: # if teammate is not self + if p.is_visible: # if teammate is visible, execute full update + self.update_other_robot(p) + elif p.state_abs_pos is not None: # otherwise update its horizontal distance (assuming last known position) + p.state_horizontal_dist = np.linalg.norm(r.loc_head_position[:2] - p.state_abs_pos[:2]) + + for p in self.opponents: + if p.is_visible: # if opponent is visible, execute full update + self.update_other_robot(p) + elif p.state_abs_pos is not None: # otherwise update its horizontal distance (assuming last known position) + p.state_horizontal_dist = np.linalg.norm(r.loc_head_position[:2] - p.state_abs_pos[:2]) + + # Update prediction of ball position/velocity + if self.play_mode_group != W.MG_OTHER: # not 'play on' nor 'game over', so ball must be stationary + self.ball_2d_pred_pos = self.ball_abs_pos[:2].copy().reshape(1, 2) + self.ball_2d_pred_vel = np.zeros((1,2)) + self.ball_2d_pred_spd = np.zeros(1) + + elif self.ball_abs_pos_last_update == self.time_local_ms: # make new prediction for new ball position (from vision or radio) + + params = np.array([*self.ball_abs_pos[:2], *np.copy(self.get_ball_abs_vel(6)[:2])], np.float32) + pred_ret = ball_predictor.predict_rolling_ball(params) + sample_no = len(pred_ret) // 5 * 2 + self.ball_2d_pred_pos = pred_ret[:sample_no].reshape(-1, 2) + self.ball_2d_pred_vel = pred_ret[sample_no:sample_no*2].reshape(-1, 2) + self.ball_2d_pred_spd = pred_ret[sample_no*2:] + + elif len(self.ball_2d_pred_pos) > 1: # otherwise, advance to next predicted step, if available + self.ball_2d_pred_pos = self.ball_2d_pred_pos[1:] + self.ball_2d_pred_vel = self.ball_2d_pred_vel[1:] + self.ball_2d_pred_spd = self.ball_2d_pred_spd[1:] + + r.update_imu(self.time_local_ms) # update imu (must be executed after localization) + + if self.ball_is_visible: + # Compute ball position, relative to torso + self.ball_rel_torso_cart_pos = r.head_to_body_part_transform("torso",self.ball_rel_head_cart_pos) + + + def update_other_robot(self,other_robot : Other_Robot): + ''' + Update other robot state based on the relative position of visible body parts + (also updated by Radio, with the exception of state_orientation) + ''' + o = other_robot + r = self.robot + + # update body parts absolute positions + o.state_body_parts_abs_pos = o.body_parts_cart_rel_pos.copy() + for bp, pos in o.body_parts_cart_rel_pos.items(): + # Using the IMU could be beneficial if we see other robots but can't self-locate + o.state_body_parts_abs_pos[bp] = r.loc_head_to_field_transform( pos, False ) + + # auxiliary variables + bps_apos = o.state_body_parts_abs_pos # read-only shortcut + bps_2d_apos_list = [v[:2] for v in bps_apos.values()] # list of body parts' 2D absolute positions + avg_2d_pt = np.average(bps_2d_apos_list, axis=0) # 2D avg pos of visible body parts + head_is_visible = 'head' in bps_apos + + # evaluate robot's state (unchanged if head is not visible) + if head_is_visible: + o.state_fallen = bps_apos['head'][2] < 0.3 + + # compute velocity if head is visible + if o.state_abs_pos is not None: + time_diff = (self.time_local_ms - o.state_last_update) / 1000 + if head_is_visible: + # if last position is 2D, we assume that the z coordinate did not change, so that v.z=0 + old_p = o.state_abs_pos if len(o.state_abs_pos)==3 else np.append(o.state_abs_pos, bps_apos['head'][2]) + velocity = (bps_apos['head'] - old_p) / time_diff + decay = o.vel_decay # neutralize decay in all axes + else: # if head is not visible, we only update the x & y components of the velocity + velocity = np.append( (avg_2d_pt - o.state_abs_pos[:2]) / time_diff, 0) + decay = (o.vel_decay,o.vel_decay,1) # neutralize decay (except in the z-axis) + # apply filter + if np.linalg.norm(velocity - o.state_filtered_velocity) < 4: # otherwise assume it was beamed + o.state_filtered_velocity /= decay # neutralize decay + o.state_filtered_velocity += o.vel_filter * (velocity-o.state_filtered_velocity) + + # compute robot's position (preferably based on head) + if head_is_visible: + o.state_abs_pos = bps_apos['head'] # 3D head position, if head is visible + else: + o.state_abs_pos = avg_2d_pt # 2D avg pos of visible body parts + + # compute robot's horizontal distance (head distance, or avg. distance of visible body parts) + o.state_horizontal_dist = np.linalg.norm(r.loc_head_position[:2] - o.state_abs_pos[:2]) + + # compute orientation based on pair of lower arms or feet, or average of both + lr_vec = None + if 'llowerarm' in bps_apos and 'rlowerarm' in bps_apos: + lr_vec = bps_apos['rlowerarm'] - bps_apos['llowerarm'] + + if 'lfoot' in bps_apos and 'rfoot' in bps_apos: + if lr_vec is None: + lr_vec = bps_apos['rfoot'] - bps_apos['lfoot'] + else: + lr_vec = (lr_vec + (bps_apos['rfoot'] - bps_apos['lfoot'])) / 2 + + if lr_vec is not None: + o.state_orientation = atan2(lr_vec[1],lr_vec[0]) * 180 / pi + 90 + + # compute projection of player area on ground (circle) + if o.state_horizontal_dist < 4: # we don't need precision if the robot is farther than 4m + max_dist = np.max(np.linalg.norm(bps_2d_apos_list - avg_2d_pt, axis=1)) + else: + max_dist = 0.2 + o.state_ground_area = (avg_2d_pt,max_dist) + + # update timestamp + o.state_last_update = self.time_local_ms \ No newline at end of file diff --git a/world/commons/Body_Part.py b/world/commons/Body_Part.py new file mode 100644 index 0000000..2187480 --- /dev/null +++ b/world/commons/Body_Part.py @@ -0,0 +1,7 @@ +from math_ops.Matrix_4x4 import Matrix_4x4 + +class Body_Part(): + def __init__(self, mass) -> None: + self.mass = float(mass) + self.joints = [] + self.transform = Matrix_4x4() # body part to head transformation matrix \ No newline at end of file diff --git a/world/commons/Draw.py b/world/commons/Draw.py new file mode 100644 index 0000000..952dc22 --- /dev/null +++ b/world/commons/Draw.py @@ -0,0 +1,345 @@ +import socket +from math_ops.Math_Ops import Math_Ops as M +import numpy as np + +class Draw(): + _socket = None + + def __init__(self, is_enabled:bool, unum:int, host:str, port:int) -> None: + self.enabled = is_enabled + self._is_team_right = None + self._unum = unum + self._prefix = f'?{unum}_'.encode() # temporary prefix that should never be used in normal circumstances + + #Create one socket for all instances + if Draw._socket is None: + Draw._socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM ) + Draw._socket.connect((host, port)) + Draw.clear_all() + + + def set_team_side(self, is_right): + ''' Called by world parser to switch side ''' + ''' + Generate an appropriate player ID + RoboViz has a bug/feature: we send "swap buffers for player: 'l_1' and RoboViz + will swap every buffer that contains 'l_1' in the name, including + 'l_10' and 'l_11'. To avoid that, we swap the separator to 'l-10', 'l-11' + ''' + self._is_team_right = is_right + self._prefix = f"{'r' if is_right else 'l'}{'_' if self._unum < 10 else '-'}{self._unum}_".encode() #e.g. b'l_5', b'l-10' + + + @staticmethod + def _send(msg, id, flush): + ''' Private method to send message if RoboViz is accessible ''' + try: + if flush: + Draw._socket.send(msg + id + b'\x00\x00\x00' + id + b'\x00') + else: + Draw._socket.send(msg + id + b'\x00') + except ConnectionRefusedError: + pass + + + def circle(self, pos2d, radius, thickness, color:bytes, id:str, flush=True): + ''' + Draw circle + + Examples + ---------- + Circle in 2D (z=0): circle((-1,2), 3, 2, Draw.Color.red, "my_circle") + ''' + if not self.enabled: return + assert type(color)==bytes, "The RGB color must be a bytes object, e.g. red: b'\xFF\x00\x00'" + assert not np.isnan(pos2d).any(), "Argument 'pos2d' contains 'nan' values" + + if self._is_team_right: + pos2d = (-pos2d[0],-pos2d[1]) + + msg = b'\x01\x00' + ( + f'{f"{pos2d[0] :.4f}":.6s}' + f'{f"{pos2d[1] :.4f}":.6s}' + f'{f"{radius :.4f}":.6s}' + f'{f"{thickness :.4f}":.6s}').encode() + color + + Draw._send(msg, self._prefix + id.encode(), flush) + + + def line(self, p1, p2, thickness, color:bytes, id:str, flush=True): + ''' + Draw line + + Examples + ---------- + Line in 3D: line((0,0,0), (0,0,2), 3, Draw.Color.red, "my_line") + Line in 2D (z=0): line((0,0), (0,1), 3, Draw.Color.red, "my_line") + ''' + if not self.enabled: return + assert type(color)==bytes, "The RGB color must be a bytes object, e.g. red: b'\xFF\x00\x00'" + assert not np.isnan(p1).any(), "Argument 'p1' contains 'nan' values" + assert not np.isnan(p2).any(), "Argument 'p2' contains 'nan' values" + + z1 = p1[2] if len(p1)==3 else 0 + z2 = p2[2] if len(p2)==3 else 0 + + if self._is_team_right: + p1 = (-p1[0],-p1[1],p1[2]) if len(p1)==3 else (-p1[0],-p1[1]) + p2 = (-p2[0],-p2[1],p2[2]) if len(p2)==3 else (-p2[0],-p2[1]) + + msg = b'\x01\x01' + ( + f'{f"{p1[0] :.4f}":.6s}' + f'{f"{p1[1] :.4f}":.6s}' + f'{f"{z1 :.4f}":.6s}' + f'{f"{p2[0] :.4f}":.6s}' + f'{f"{p2[1] :.4f}":.6s}' + f'{f"{z2 :.4f}":.6s}' + f'{f"{thickness :.4f}":.6s}').encode() + color + + Draw._send(msg, self._prefix + id.encode(), flush) + + + def point(self, pos, size, color:bytes, id:str, flush=True): + ''' + Draw point + + Examples + ---------- + Point in 3D: point((1,1,1), 3, Draw.Color.red, "my_point") + Point in 2D (z=0): point((1,1), 3, Draw.Color.red, "my_point") + ''' + if not self.enabled: return + assert type(color)==bytes, "The RGB color must be a bytes object, e.g. red: b'\xFF\x00\x00'" + assert not np.isnan(pos).any(), "Argument 'pos' contains 'nan' values" + + z = pos[2] if len(pos)==3 else 0 + + if self._is_team_right: + pos = (-pos[0],-pos[1],pos[2]) if len(pos)==3 else (-pos[0],-pos[1]) + + msg = b'\x01\x02' + ( + f'{f"{pos[0] :.4f}":.6s}' + f'{f"{pos[1] :.4f}":.6s}' + f'{f"{z :.4f}":.6s}' + f'{f"{size :.4f}":.6s}').encode() + color + + Draw._send(msg, self._prefix + id.encode(), flush) + + + def sphere(self, pos, radius, color:bytes, id:str, flush=True): + ''' + Draw sphere + + Examples + ---------- + Sphere in 3D: sphere((1,1,1), 3, Draw.Color.red, "my_sphere") + Sphere in 2D (z=0): sphere((1,1), 3, Draw.Color.red, "my_sphere") + ''' + if not self.enabled: return + assert type(color)==bytes, "The RGB color must be a bytes object, e.g. red: b'\xFF\x00\x00'" + assert not np.isnan(pos).any(), "Argument 'pos' contains 'nan' values" + + z = pos[2] if len(pos)==3 else 0 + + if self._is_team_right: + pos = (-pos[0],-pos[1],pos[2]) if len(pos)==3 else (-pos[0],-pos[1]) + + msg = b'\x01\x03' + ( + f'{f"{pos[0] :.4f}":.6s}' + f'{f"{pos[1] :.4f}":.6s}' + f'{f"{z :.4f}":.6s}' + f'{f"{radius :.4f}":.6s}').encode() + color + + Draw._send(msg, self._prefix + id.encode(), flush) + + + def polygon(self, vertices, color:bytes, alpha:int, id:str, flush=True): + ''' + Draw polygon + + Examples + ---------- + Polygon in 3D: polygon(((0,0,0),(1,0,0),(0,1,0)), Draw.Color.red, 255, "my_polygon") + ''' + if not self.enabled: return + assert type(color)==bytes, "The RGB color must be a bytes object, e.g. red: b'\xFF\x00\x00'" + assert 0<=alpha<=255, "The alpha channel (degree of opacity) must be in range [0,255]" + + if self._is_team_right: + vertices = [(-v[0],-v[1],v[2]) for v in vertices] + + msg = b'\x01\x04' + bytes([len(vertices)]) + color + alpha.to_bytes(1,'big') + + for v in vertices: + msg += ( + f'{f"{v[0] :.4f}":.6s}' + f'{f"{v[1] :.4f}":.6s}' + f'{f"{v[2] :.4f}":.6s}').encode() + + Draw._send(msg, self._prefix + id.encode(), flush) + + + def annotation(self, pos, text, color:bytes, id:str, flush=True): + ''' + Draw annotation + + Examples + ---------- + Annotation in 3D: annotation((1,1,1), "SOMEtext!", Draw.Color.red, "my_annotation") + Annotation in 2D (z=0): annotation((1,1), "SOMEtext!", Draw.Color.red, "my_annotation") + ''' + if not self.enabled: return + if type(text) != bytes: text = str(text).encode() + assert type(color)==bytes, "The RGB color must be a bytes object, e.g. red: b'\xFF\x00\x00'" + z = pos[2] if len(pos)==3 else 0 + + if self._is_team_right: + pos = (-pos[0],-pos[1],pos[2]) if len(pos)==3 else (-pos[0],-pos[1]) + + msg = b'\x02\x00' + ( + f'{f"{pos[0] :.4f}":.6s}' + f'{f"{pos[1] :.4f}":.6s}' + f'{f"{z :.4f}":.6s}').encode() + color + text + b'\x00' + + Draw._send(msg, self._prefix + id.encode(), flush) + + + def arrow(self, p1, p2, arrowhead_size, thickness, color:bytes, id:str, flush=True): + ''' + Draw arrow + + Examples + ---------- + Arrow in 3D: arrow((0,0,0), (0,0,2), 0.1, 3, Draw.Color.red, "my_arrow") + Arrow in 2D (z=0): arrow((0,0), (0,1), 0.1, 3, Draw.Color.red, "my_arrow") + ''' + if not self.enabled: return + assert type(color)==bytes, "The RGB color must be a bytes object, e.g. red: b'\xFF\x00\x00'" + + # No need to invert sides, the called shapes will handle that + if len(p1)==2: p1 = M.to_3d(p1) + else: p1 = np.asarray(p1) + if len(p2)==2: p2 = M.to_3d(p2) + else: p2 = np.asarray(p2) + + vec = p2-p1 + vec_size = np.linalg.norm(vec) + if vec_size == 0: return #return without warning/error + if arrowhead_size > vec_size: arrowhead_size = vec_size + + ground_proj_perpendicular = np.array([ vec[1], -vec[0], 0 ]) + + if np.all(ground_proj_perpendicular == 0): #vertical arrow + ground_proj_perpendicular = np.array([ arrowhead_size/2, 0, 0 ]) + else: + ground_proj_perpendicular *= arrowhead_size/2 / np.linalg.norm(ground_proj_perpendicular) + + head_start = p2 - vec * (arrowhead_size/vec_size) + head_pt1 = head_start + ground_proj_perpendicular + head_pt2 = head_start - ground_proj_perpendicular + + self.line(p1,p2,thickness,color,id,False) + self.line(p2,head_pt1,thickness,color,id,False) + self.line(p2,head_pt2,thickness,color,id,flush) + + + def flush(self, id): + ''' Flush specific drawing by ID ''' + if not self.enabled: return + + Draw._send(b'\x00\x00', self._prefix + id.encode(), False) + + def clear(self, id): + ''' Clear specific drawing by ID ''' + if not self.enabled: return + + Draw._send(b'\x00\x00', self._prefix + id.encode(), True) #swap buffer twice + + + def clear_player(self): + ''' Clear all drawings made by this player ''' + if not self.enabled: return + + Draw._send(b'\x00\x00', self._prefix, True) #swap buffer twice + + + @staticmethod + def clear_all(): + ''' Clear all drawings of all players ''' + if Draw._socket is not None: + Draw._send(b'\x00\x00\x00\x00\x00',b'',False) #swap buffer twice using no id + + + class Color(): + ''' + Based on X11 colors + The names are restructured to make better suggestions + ''' + pink_violet = b'\xC7\x15\x85' + pink_hot = b'\xFF\x14\x93' + pink_violet_pale = b'\xDB\x70\x93' + pink = b'\xFF\x69\xB4' + pink_pale = b'\xFF\xB6\xC1' + + red_dark = b'\x8B\x00\x00' + red = b'\xFF\x00\x00' + red_brick = b'\xB2\x22\x22' + red_crimson = b'\xDC\x14\x3C' + red_indian = b'\xCD\x5C\x5C' + red_salmon = b'\xFA\x80\x72' + + orange_red = b'\xFF\x45\x00' + orange = b'\xFF\x8C\x00' + orange_ligth = b'\xFF\xA5\x00' + + yellow_gold = b'\xFF\xD7\x00' + yellow = b'\xFF\xFF\x00' + yellow_light = b'\xBD\xB7\x6B' + + brown_maroon =b'\x80\x00\x00' + brown_dark = b'\x8B\x45\x13' + brown = b'\xA0\x52\x2D' + brown_gold = b'\xB8\x86\x0B' + brown_light = b'\xCD\x85\x3F' + brown_pale = b'\xDE\xB8\x87' + + green_dark = b'\x00\x64\x00' + green = b'\x00\x80\x00' + green_lime = b'\x32\xCD\x32' + green_light = b'\x00\xFF\x00' + green_lawn = b'\x7C\xFC\x00' + green_pale = b'\x90\xEE\x90' + + cyan_dark = b'\x00\x80\x80' + cyan_medium = b'\x00\xCE\xD1' + cyan = b'\x00\xFF\xFF' + cyan_light = b'\xAF\xEE\xEE' + + blue_dark = b'\x00\x00\x8B' + blue = b'\x00\x00\xFF' + blue_royal = b'\x41\x69\xE1' + blue_medium = b'\x1E\x90\xFF' + blue_light = b'\x00\xBF\xFF' + blue_pale = b'\x87\xCE\xEB' + + purple_violet = b'\x94\x00\xD3' + purple_magenta = b'\xFF\x00\xFF' + purple_light = b'\xBA\x55\xD3' + purple_pale = b'\xDD\xA0\xDD' + + white = b'\xFF\xFF\xFF' + gray_10 = b'\xE6\xE6\xE6' + gray_20 = b'\xCC\xCC\xCC' + gray_30 = b'\xB2\xB2\xB2' + gray_40 = b'\x99\x99\x99' + gray_50 = b'\x80\x80\x80' + gray_60 = b'\x66\x66\x66' + gray_70 = b'\x4C\x4C\x4C' + gray_80 = b'\x33\x33\x33' + gray_90 = b'\x1A\x1A\x1A' + black = b'\x00\x00\x00' + + @staticmethod + def get(r,g,b): + ''' Get RGB color (0-255) ''' + return bytes([int(r),int(g),int(b)]) diff --git a/world/commons/Joint_Info.py b/world/commons/Joint_Info.py new file mode 100644 index 0000000..5fac4f1 --- /dev/null +++ b/world/commons/Joint_Info.py @@ -0,0 +1,24 @@ +import numpy as np + +class Joint_Info(): + def __init__(self, xml_element) -> None: + self.perceptor = xml_element.attrib['perceptor'] + self.effector = xml_element.attrib['effector'] + self.axes = np.array([ + float(xml_element.attrib['xaxis']), + float(xml_element.attrib['yaxis']), + float(xml_element.attrib['zaxis'])]) + self.min = int(xml_element.attrib['min']) + self.max = int(xml_element.attrib['max']) + + self.anchor0_part = xml_element[0].attrib['part'] + self.anchor0_axes = np.array([ + float(xml_element[0].attrib['y']), + float(xml_element[0].attrib['x']), + float(xml_element[0].attrib['z'])]) #x and y axes are switched + + self.anchor1_part = xml_element[1].attrib['part'] + self.anchor1_axes_neg = np.array([ + -float(xml_element[1].attrib['y']), + -float(xml_element[1].attrib['x']), + -float(xml_element[1].attrib['z'])]) #x and y axes are switched diff --git a/world/commons/Other_Robot.py b/world/commons/Other_Robot.py new file mode 100644 index 0000000..5b17763 --- /dev/null +++ b/world/commons/Other_Robot.py @@ -0,0 +1,28 @@ +import numpy as np + +#Note: When other robot is seen, all previous body part positions are deleted +# E.g. we see 5 body parts at 0 seconds -> body_parts_cart_rel_pos contains 5 elements +# we see 1 body part at 1 seconds -> body_parts_cart_rel_pos contains 1 element + + +class Other_Robot(): + def __init__(self, unum, is_teammate) -> None: + self.unum = unum # convenient variable to indicate uniform number (same as other robot's index + 1) + self.is_self = False # convenient flag to indicate if this robot is self + self.is_teammate = is_teammate # convenient variable to indicate if this robot is from our team + self.is_visible = False # True if this robot was seen in the last message from the server (it doesn't mean we know its absolute location) + self.body_parts_cart_rel_pos = dict() # cartesian relative position of the robot's visible body parts + self.body_parts_sph_rel_pos = dict() # spherical relative position of the robot's visible body parts + self.vel_filter = 0.3 # EMA filter coefficient applied to self.state_filtered_velocity + self.vel_decay = 0.95 # velocity decay at every vision cycle (neutralized if velocity is updated) + + + # State variables: these are computed when this robot is visible and when the original robot is able to self-locate + self.state_fallen = False # true if the robot is lying down (updated when head is visible) + self.state_last_update = 0 # World.time_local_ms when the state was last updated + self.state_horizontal_dist = 0 # horizontal head distance if head is visible, otherwise, average horizontal distance of visible body parts (the distance is updated by vision or radio when state_abs_pos gets a new value, but also when the other player is not visible, by assuming its last position) + self.state_abs_pos = None # 3D head position if head is visible, otherwise, 2D average position of visible body parts, or, 2D radio head position + self.state_orientation = 0 # orientation based on pair of lower arms or feet, or average of both (WARNING: may be older than state_last_update) + self.state_ground_area = None # (pt_2d,radius) projection of player area on ground (circle), not precise if farther than 3m (for performance), useful for obstacle avoidance when it falls + self.state_body_parts_abs_pos = dict() # 3D absolute position of each body part + self.state_filtered_velocity = np.zeros(3) # 3D filtered velocity (m/s) (if the head is not visible, the 2D part is updated and v.z decays) diff --git a/world/commons/Path_Manager.py b/world/commons/Path_Manager.py new file mode 100644 index 0000000..7cc3443 --- /dev/null +++ b/world/commons/Path_Manager.py @@ -0,0 +1,583 @@ +from cpp.a_star import a_star +from math_ops.Math_Ops import Math_Ops as M +from world.World import World +import math +import numpy as np + + +class Path_Manager(): + MODE_CAUTIOUS = 0 + MODE_DRIBBLE = 1 # safety margins are increased + MODE_AGGRESSIVE = 2 # safety margins are reduced for opponents + + STATUS_SUCCESS = 0 # the pathfinding algorithm was executed normally + STATUS_TIMEOUT = 1 # timeout before the target was reached (may be impossible) + STATUS_IMPOSSIBLE = 2 # impossible to reach target (all options were tested) + STATUS_DIRECT = 3 # no obstacles between start and target (path contains only 2 points: the start and target) + + HOT_START_DIST_WALK = 0.05 # hot start prediction distance (when walking) + HOT_START_DIST_DRIBBLE = 0.10 # hot start prediction distance (when dribbling) + + def __init__(self, world : World) -> None: + self.world = world + + self._draw_obstacles = False # enabled by function 'draw_options' + self._draw_path = False # enabled by function 'draw_options' + self._use_team_channel = False # enabled by function 'draw_options' + + # internal variables to bootstrap the path to start from a prediction (to reduce path instability) + self.last_direction_rad = None + self.last_update = 0 + self.last_start_dist = None + + def draw_options(self, enable_obstacles, enable_path, use_team_drawing_channel=False): + ''' + Enable or disable drawings, and change drawing channel + If self.world.draw.enable is False, these options are ignored + + Parameters + ---------- + enable_obstacles : bool + draw relevant obstacles for path planning + enable_path : bool + draw computed path + use_team_drawing_channel : bool + True to use team drawing channel, otherwise use individual channel + Using individual channels for each player means that drawings with the same name can coexist + With the team channel, drawings with the same name will replace previous drawings, even if drawn by a teammate + ''' + self._draw_obstacles = enable_obstacles + self._draw_path = enable_path + self._use_team_channel = use_team_drawing_channel + + def get_obstacles(self, include_teammates, include_opponents, include_play_mode_restrictions, max_distance = 4, max_age = 500, + ball_safety_margin = 0, goalpost_safety_margin = 0, mode = MODE_CAUTIOUS, priority_unums=[]): + ''' + Parameters + ---------- + include_teammates : bool + include teammates in the returned list of obstacles + include_opponents : bool + include opponents in the returned list of obstacles + max_distance : float + teammates or opponents are only considered if they are closer than `max_distance` (meters) + max_age : float + teammates or opponents are only considered if they were seen in the last `max_age` (milliseconds) + ball_safety_margin : float + minimum value for the ball's soft repulsion radius + this value is increased when the game is stopped, and when the ball is almost out of bounds + default is zero, the ball is ignored + goalpost_safety_margin : float + hard repulsion radius around the opponents' goalposts + default is zero, uses the minimum margin + mode : int + overall attitude towards safety margins (concerns teammates and opponents) + priority_unums : list + list of teammates to avoid (since their role is more important) + + Returns + ------- + obstacles : list + list of obstacles, where each obstacle is a tuple of 5 floats (x, y, hard radius, soft radius, repulsive force) + ''' + w = self.world + + ball_2d = w.ball_abs_pos[:2] + obstacles = [] + + # 'comparator' is a variable local to the lambda, which captures the current value of (w.time_local_ms - max_age) + check_age = lambda last_update, comparator = w.time_local_ms - max_age : last_update > 0 and last_update >= comparator + + #---------------------------------------------- Get recently seen close teammates + if include_teammates: + soft_radius = 1.1 if mode == Path_Manager.MODE_DRIBBLE else 0.6 # soft radius: repulsive force is max at center and fades + + def get_hard_radius(t): + if t.unum in priority_unums: + return 1.0 # extra distance for priority roles + else: + return t.state_ground_area[1]+0.2 + + # Get close teammates (center, hard radius, soft radius, force) + obstacles.extend( (*t.state_ground_area[0], + get_hard_radius(t), + 1.5 if t.unum in priority_unums else soft_radius, + 1.0) # repulsive force + for t in w.teammates if not t.is_self and check_age(t.state_last_update) and t.state_horizontal_dist < max_distance) + + #---------------------------------------------- Get recently seen close opponents + if include_opponents: + + # soft radius: repulsive force is max at center and fades + if mode == Path_Manager.MODE_AGGRESSIVE: + soft_radius = 0.6 + hard_radius = lambda o : 0.2 + elif mode == Path_Manager.MODE_DRIBBLE: + soft_radius = 2.3 + hard_radius = lambda o : o.state_ground_area[1]+0.9 + else: + soft_radius = 1.0 + hard_radius = lambda o : o.state_ground_area[1]+0.2 + + # Get close opponents (center, hard radius, soft radius, force) + obstacles.extend( (*o.state_ground_area[0], + hard_radius(o), + soft_radius, + 1.5 if o.unum == 1 else 1.0) # repulsive force (extra for their GK) + for o in w.opponents if o.state_last_update > 0 and w.time_local_ms - o.state_last_update <= max_age and o.state_horizontal_dist < max_distance) + + #---------------------------------------------- Get play mode restrictions + if include_play_mode_restrictions: + if w.play_mode == World.M_THEIR_GOAL_KICK: + obstacles.extend((15,i,2.1,0,0) for i in range(-2,3)) # 5 circular obstacles to cover their goal area + elif w.play_mode == World.M_THEIR_PASS: + obstacles.append((*ball_2d, 1.2, 0, 0)) + elif w.play_mode in [World.M_THEIR_KICK_IN,World.M_THEIR_CORNER_KICK,World.M_THEIR_FREE_KICK,World.M_THEIR_DIR_FREE_KICK, World.M_THEIR_OFFSIDE]: + obstacles.append((*ball_2d, 2.5, 0, 0)) + + #---------------------------------------------- Get ball + if ball_safety_margin > 0: + + # increase ball safety margin in certain game scenarios + if (w.play_mode_group != w.MG_OTHER) or abs(ball_2d[1])>9.5 or abs(ball_2d[0])>14.5: + ball_safety_margin += 0.12 + + obstacles.append((*ball_2d, 0, ball_safety_margin, 8)) + + #---------------------------------------------- Get goal posts + if goalpost_safety_margin > 0: + obstacles.append((14.75, 1.10,goalpost_safety_margin,0,0)) + obstacles.append((14.75,-1.10,goalpost_safety_margin,0,0)) + + #---------------------------------------------- Draw obstacles + if self._draw_obstacles: + d = w.team_draw if self._use_team_channel else w.draw + if d.enabled: + for o in obstacles: + if o[3] > 0: d.circle(o[:2],o[3],o[4]/2, d.Color.orange, "path_obstacles", False) + if o[2] > 0: d.circle(o[:2],o[2],1, d.Color.red, "path_obstacles", False) + d.flush("path_obstacles") + + return obstacles + + def _get_hot_start(self, start_distance): + ''' + Get hot start position for path (considering the previous path) + (as opposed to a cold start, where the path starts at the player) + ''' + if self.last_update > 0 and self.world.time_local_ms - self.last_update == 20 and self.last_start_dist == start_distance: + return self.world.robot.loc_head_position[:2] + M.vector_from_angle(self.last_direction_rad, is_rad = True) * start_distance + else: + return self.world.robot.loc_head_position[:2] # return cold start if start_distance was different or the position was not updated in the last step + + def _update_hot_start(self, next_dir_rad, start_distance): + ''' Update hot start position for next run ''' + self.last_direction_rad = next_dir_rad + self.last_update = self.world.time_local_ms + self.last_start_dist = start_distance + + def _extract_target_from_path(self, path, path_len, ret_segments): + ret_seg_ceil = math.ceil(ret_segments) + + if path_len >= ret_seg_ceil: + i = ret_seg_ceil * 2 # path index of ceil point (x) + if ret_seg_ceil == ret_segments: + return path[i:i+2] + else: + floor_w = ret_seg_ceil-ret_segments + return path[i-2:i] * floor_w + path[i:i+2] * (1-floor_w) + else: + return path[-2:] # path end + + + def get_path_to_ball(self, x_ori = None, x_dev = -0.2, y_dev = 0, torso_ori = None, torso_ori_thrsh = 1, + priority_unums:list=[], is_aggressive=True, safety_margin = 0.25, timeout = 3000): + ''' + Get next target from path to ball (next absolute position + next absolute orientation) + If the robot is an active player, and close to the ball, it makes sense to be aggressive + If the robot is far, it should follow the role_position instead to predict the intersection with ball + + + Parameters + ---------- + x_ori : float + (This variable allows the specification of a target position, relative to the ball, in a custom reference frame.) + absolute orientation of the custom reference frame's x-axis + if None, the orientation is given by the vector (robot->ball) + x_dev : float + (This variable allows the specification of a target position, relative to the ball, in a custom reference frame.) + target position deviation, in the custom reference frame's x-axis + y_dev : float + (This variable allows the specification of a target position, relative to the ball, in a custom reference frame.) + target position deviation, in the custom reference frame's y-axis + torso_ori : float + torso's target absolute orientation (see `torso_ori_thrsh`) + if None, the orientation is given by the vector (robot->target) + torso_ori_thrsh : float + `torso_ori` will only be applied when the distance between robot and final target is < `torso_ori_thrsh` meters + otherwise, the robot will orient itself towards the final target + priority_unums : list + list of teammates to avoid (since their role is more important) + is_aggressive : bool + if True, safety margins are reduced for opponents + safety_margin : float + repulsion radius around ball to avoid colliding with it + timeout : float + maximum execution time (in microseconds) + + Returns + ------- + next_pos : ndarray + next absolute position from path to ball + next_ori : float + next absolute orientation + distance : float + minimum between (distance to final target) and (distance to ball) + + + Example + ------- + ---------------------------------------------------------------------------------------------- + x_ori | x_dev | y_dev | torso_ori | OBS + -------------+---------+---------+-------------+---------------------------------------------- + None => | - | !0 | - | Not recommended. Will not converge. + (orient. of: | 0 | 0 | None | Frontal ball chase, expected* slow approach + robot->ball) | 0 | 0 | value | Oriented ball chase, expected* slow approach + | >0 | 0 | - | Not recommended. Will not converge. + | <0 | 0 | None | Frontal ball chase until distance == x_dev + | <0 | 0 | value | Oriented ball chase until distance == x_dev + -------------+---------+---------+-------------+---------------------------------------------- + value | - | - | None | Frontal point chase + | - | - | value | Oriented point chase + ---------------------------------------------------------------------------------------------- + * it depends on the caller function (expected slow walking near target) + `torso_ori` will only be applied when the distance between robot and final target is < `torso_ori_thrsh` meters + ''' + + w = self.world + r = w.robot + dev = np.array([x_dev,y_dev]) + dev_len = np.linalg.norm(dev) + dev_mult = 1 + + # use ball prediction if we are further than 0.5 m and in PlayOn + if np.linalg.norm(w.ball_abs_pos[:2] - r.loc_head_position[:2]) > 0.5 and w.play_mode_group == w.MG_OTHER: + ball_2d = w.get_intersection_point_with_ball(0.4)[0] # intersection point, while moving at 0.4 m/s + else: + ball_2d = w.ball_abs_pos[:2] + + # custom reference frame orientation + vec_me_ball = ball_2d - r.loc_head_position[:2] + if x_ori is None: + x_ori = M.vector_angle(vec_me_ball) + + distance_boost = 0 # boost for returned distance to target + if torso_ori is not None and dev_len > 0: + approach_ori_diff = abs(M.normalize_deg( r.imu_torso_orientation - torso_ori )) + if approach_ori_diff > 15: # increase walking speed near target if robot is far from approach orientation + distance_boost = 0.15 + if approach_ori_diff > 30: # increase target distance to ball if robot is far from approach orientation + dev_mult = 1.3 + if approach_ori_diff > 45: # increase safety margin around ball if robot is far from approach orientation + safety_margin = max(0.32,safety_margin) + + #------------------------------------------- get target + + front_unit_vec = M.vector_from_angle(x_ori) + left_unit_vec = np.array([-front_unit_vec[1], front_unit_vec[0]]) + + rel_target = front_unit_vec * dev[0] + left_unit_vec * dev[1] + target = ball_2d + rel_target * dev_mult + target_vec = target - r.loc_head_position[:2] + target_dist = np.linalg.norm(target_vec) + + if self._draw_path: + d = self.world.team_draw if self._use_team_channel else self.world.draw + d.point(target, 4, d.Color.red, "path_target") # will not draw if drawing object is internally disabled + + #------------------------------------------- get obstacles + + # Ignore ball if we are on the same side of the target (with small margin) + if dev_len>0 and np.dot(vec_me_ball, rel_target) < -0.10: + safety_margin = 0 + + obstacles = self.get_obstacles(include_teammates = True, include_opponents = True, include_play_mode_restrictions = True, + ball_safety_margin = safety_margin, + mode = Path_Manager.MODE_AGGRESSIVE if is_aggressive else Path_Manager.MODE_CAUTIOUS, + priority_unums = priority_unums) + + # Add obstacle on the side opposite to the target + if dev_len>0 and safety_margin > 0: + center = ball_2d - M.normalize_vec( rel_target ) * safety_margin + obstacles.append((*center, 0, safety_margin*0.9, 5)) + if self._draw_obstacles: + d = w.team_draw if self._use_team_channel else w.draw + if d.enabled: + d.circle(center,safety_margin*0.8,2.5, d.Color.orange, "path_obstacles_1") + + #------------------------------------------- get path + + # see explanation for the context at the hot start update section below + start_pos = self._get_hot_start(Path_Manager.HOT_START_DIST_WALK) if target_dist > 0.4 else self.world.robot.loc_head_position[:2] + + path, path_len, path_status, path_cost = self.get_path(start_pos, True, obstacles, target, timeout) + path_end = path[-2:] # last position allowed by A* + + #------------------------------------------- get relevant distances + + if w.ball_last_seen > w.time_local_ms - w.VISUALSTEP_MS: # ball is in FOV + raw_ball_dist = np.linalg.norm(w.ball_rel_torso_cart_pos[:2]) # - distance between torso center and ball center + else: # otherwise use absolute coordinates to compute distance + raw_ball_dist = np.linalg.norm(vec_me_ball) # - distance between head center and ball center + + avoid_touching_ball = (w.play_mode_group != w.MG_OTHER) + distance_to_final_target = np.linalg.norm(path_end - r.loc_head_position[:2]) + distance_to_ball = max(0.07 if avoid_touching_ball else 0.14, raw_ball_dist - 0.13) + caution_dist = min(distance_to_ball,distance_to_final_target) + + #------------------------------------------- get next target position + + next_pos = self._extract_target_from_path( path, path_len, ret_segments=1 if caution_dist < 1 else 2 ) + + #------------------------------------------ get next target orientation + + # use given orientation if it exists, else target's orientation if far enough, else current orientation + if torso_ori is not None: + + if caution_dist > torso_ori_thrsh: + next_ori = M.vector_angle(target_vec) + else: + mid_ori = M.normalize_deg( M.vector_angle(vec_me_ball) - M.vector_angle(-dev) - x_ori + torso_ori ) + mid_ori_diff = abs(M.normalize_deg(mid_ori - r.imu_torso_orientation)) + final_ori_diff = abs(M.normalize_deg(torso_ori - r.imu_torso_orientation)) + next_ori = mid_ori if mid_ori_diff + 10 < final_ori_diff else torso_ori + + elif target_dist > 0.1: + next_ori = M.vector_angle(target_vec) + else: + next_ori = r.imu_torso_orientation + + #------------------------------------------ update hot start for next run + + ''' Defining the hot start distance: + - if path_len is zero, there is no hot start, because we are already there (dist=0) + - if the target is close, the hot start is not applied (see above) + - if the next pos is very close (due to hard obstacle), the hot start is the next pos (disttarget) + priority_unums : list + list of teammates to avoid (since their role is more important) + is_aggressive : bool + if True, safety margins are reduced for opponents + timeout : float + maximum execution time (in microseconds) + ''' + + w = self.world + + #------------------------------------------- get target + + target_vec = target - w.robot.loc_head_position[:2] + target_dist = np.linalg.norm(target_vec) + + #------------------------------------------- get obstacles + + obstacles = self.get_obstacles(include_teammates = True, include_opponents = True, include_play_mode_restrictions = True, + mode = Path_Manager.MODE_AGGRESSIVE if is_aggressive else Path_Manager.MODE_CAUTIOUS, priority_unums = priority_unums) + + #------------------------------------------- get path + + # see explanation for the context at the hot start update section below + start_pos = self._get_hot_start(Path_Manager.HOT_START_DIST_WALK) if target_dist > 0.4 else self.world.robot.loc_head_position[:2] + + path, path_len, path_status, path_cost = self.get_path(start_pos, True, obstacles, target, timeout) + path_end = path[-2:] # last position allowed by A* + + #------------------------------------------- get next target position + next_pos = self._extract_target_from_path(path, path_len, ret_segments) + + #------------------------------------------ get next target orientation + + # use given orientation if it exists, else target's orientation if far enough, else current orientation + if torso_ori is not None: + next_ori = torso_ori + elif target_dist > 0.1: + next_ori = M.vector_angle(target_vec) + else: + next_ori = w.robot.imu_torso_orientation + + #------------------------------------------ update hot start for next run + + ''' Defining the hot start distance: + - if path_len is zero, there is no hot start, because we are already there (dist=0) + - if the target is close, the hot start is not applied (see above) + - if the next pos is very close (due to hard obstacle), the hot start is the next pos (dist + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/world/commons/robots/nao1.xml b/world/commons/robots/nao1.xml new file mode 100644 index 0000000..f6297a7 --- /dev/null +++ b/world/commons/robots/nao1.xml @@ -0,0 +1,161 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/world/commons/robots/nao2.xml b/world/commons/robots/nao2.xml new file mode 100644 index 0000000..59a279c --- /dev/null +++ b/world/commons/robots/nao2.xml @@ -0,0 +1,164 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/world/commons/robots/nao3.xml b/world/commons/robots/nao3.xml new file mode 100644 index 0000000..bb35848 --- /dev/null +++ b/world/commons/robots/nao3.xml @@ -0,0 +1,161 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/world/commons/robots/nao4.xml b/world/commons/robots/nao4.xml new file mode 100644 index 0000000..e728ff0 --- /dev/null +++ b/world/commons/robots/nao4.xml @@ -0,0 +1,175 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +