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 0000000..7d0e050
Binary files /dev/null and b/behaviors/custom/Dribble/dribble_R0.pkl differ
diff --git a/behaviors/custom/Dribble/dribble_R1.pkl b/behaviors/custom/Dribble/dribble_R1.pkl
new file mode 100644
index 0000000..9e1043c
Binary files /dev/null and b/behaviors/custom/Dribble/dribble_R1.pkl differ
diff --git a/behaviors/custom/Dribble/dribble_R2.pkl b/behaviors/custom/Dribble/dribble_R2.pkl
new file mode 100644
index 0000000..9d9d61f
Binary files /dev/null and b/behaviors/custom/Dribble/dribble_R2.pkl differ
diff --git a/behaviors/custom/Dribble/dribble_R3.pkl b/behaviors/custom/Dribble/dribble_R3.pkl
new file mode 100644
index 0000000..aa33258
Binary files /dev/null and b/behaviors/custom/Dribble/dribble_R3.pkl differ
diff --git a/behaviors/custom/Dribble/dribble_R4.pkl b/behaviors/custom/Dribble/dribble_R4.pkl
new file mode 100644
index 0000000..781d55a
Binary files /dev/null and b/behaviors/custom/Dribble/dribble_R4.pkl differ
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 0000000..d7976e4
Binary files /dev/null and b/behaviors/custom/Fall/fall.pkl differ
diff --git a/behaviors/custom/Get_Up/Get_Up.py b/behaviors/custom/Get_Up/Get_Up.py
new file mode 100644
index 0000000..8dd88ac
--- /dev/null
+++ b/behaviors/custom/Get_Up/Get_Up.py
@@ -0,0 +1,68 @@
+from agent.Base_Agent import Base_Agent
+from collections import deque
+import numpy as np
+
+class Get_Up():
+
+ def __init__(self, base_agent : Base_Agent) -> 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 0000000..ac7ec3e
Binary files /dev/null and b/behaviors/custom/Walk/walk_R0.pkl differ
diff --git a/behaviors/custom/Walk/walk_R1_R3.pkl b/behaviors/custom/Walk/walk_R1_R3.pkl
new file mode 100644
index 0000000..5e56c0e
Binary files /dev/null and b/behaviors/custom/Walk/walk_R1_R3.pkl differ
diff --git a/behaviors/custom/Walk/walk_R2.pkl b/behaviors/custom/Walk/walk_R2.pkl
new file mode 100644
index 0000000..bde7460
Binary files /dev/null and b/behaviors/custom/Walk/walk_R2.pkl differ
diff --git a/behaviors/custom/Walk/walk_R4.pkl b/behaviors/custom/Walk/walk_R4.pkl
new file mode 100644
index 0000000..2d86dbc
Binary files /dev/null and b/behaviors/custom/Walk/walk_R4.pkl differ
diff --git a/behaviors/slot/common/Dive_Left.xml b/behaviors/slot/common/Dive_Left.xml
new file mode 100644
index 0000000..e4e239e
--- /dev/null
+++ b/behaviors/slot/common/Dive_Left.xml
@@ -0,0 +1,71 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+