MagDish
/
Dribble
0
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

first commit

Browse Source
main
Her-darling 7 months ago
parent 979d9521a5
commit b7d16e74a5
145 changed files with 20499 additions and 0 deletions
Show Stats Download Patch File Download Diff File

8
.idea/.gitignore vendored
Unescape Escape View File

@ -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

12
.idea/FCPCodebase.iml
Unescape Escape View File

@ -0,0 +1,12 @@
<?xml version="1.0" encoding="UTF-8"?>
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager">
<content url="file://$MODULE_DIR$" />
<orderEntry type="inheritedJdk" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
<component name="PyDocumentationSettings">
<option name="format" value="PLAIN" />
<option name="myDocStringFormat" value="Plain" />
</component>
</module>

19
.idea/inspectionProfiles/Project_Default.xml
Unescape Escape View File

@ -0,0 +1,19 @@
<component name="InspectionProjectProfileManager">
<profile version="1.0">
<option name="myName" value="Project Default" />
<inspection_tool class="PyPep8NamingInspection" enabled="true" level="WEAK WARNING" enabled_by_default="true">
<option name="ignoredErrors">
<list>
<option value="N806" />
</list>
</option>
</inspection_tool>
<inspection_tool class="PyUnresolvedReferencesInspection" enabled="true" level="WARNING" enabled_by_default="true">
<option name="ignoredIdentifiers">
<list>
<option value="list.copy" />
</list>
</option>
</inspection_tool>
</profile>
</component>

6
.idea/inspectionProfiles/profiles_settings.xml
Unescape Escape View File

@ -0,0 +1,6 @@
<component name="InspectionProjectProfileManager">
<settings>
<option name="USE_PROJECT_PROFILE" value="false" />
<version value="1.0" />
</settings>
</component>

7
.idea/misc.xml
Unescape Escape View File

@ -0,0 +1,7 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="Black">
<option name="sdkName" value="fcp_env" />
</component>
<component name="ProjectRootManager" version="2" project-jdk-name="FCPCodebase" project-jdk-type="Python SDK" />
</project>

8
.idea/modules.xml
Unescape Escape View File

@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/FCPCodebase.iml" filepath="$PROJECT_DIR$/.idea/FCPCodebase.iml" />
</modules>
</component>
</project>

6
.idea/vcs.xml
Unescape Escape View File

@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="" vcs="Git" />
</component>
</project>

674
LICENSE
Unescape Escape View File

@ -0,0 +1,674 @@
GNU GENERAL PUBLIC LICENSE
Version 3, 29 June 2007
Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
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.
<one line to give the program's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
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 <https://www.gnu.org/licenses/>.
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:
<program> Copyright (C) <year> <name of author>
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
<https://www.gnu.org/licenses/>.
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
<https://www.gnu.org/licenses/why-not-lgpl.html>.

36
One_vs_One.py
Unescape Escape View File

@ -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()

13
Run_Full_Team.py
Unescape Escape View File

@ -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()

14
Run_One_vs_One.py
Unescape Escape View File

@ -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()

18
Run_Player.py
Unescape Escape View File

@ -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()

96
Run_Utils.py
Unescape Escape View File

@ -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()

35
T_vs_T.py
Unescape Escape View File

@ -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()

271
agent/Agent.py
Unescape Escape View File

@ -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})")

88
agent/Agent_Penalty.py
Unescape Escape View File

@ -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")

47
agent/Base_Agent.py
Unescape Escape View File

@ -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 = []

183
behaviors/Behavior.py
Unescape Escape View File

@ -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)

105
behaviors/Head.py
Unescape Escape View File

@ -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 )

97
behaviors/Poses.py
Unescape Escape View File

@ -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)

110
behaviors/Slot_Engine.py
Unescape Escape View File

@ -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

74
behaviors/custom/Basic_Kick/Basic_Kick.py
Unescape Escape View File

@ -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

207
behaviors/custom/Dribble/Dribble.py
Unescape Escape View File

@ -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 a1<a2:
if a1 <= cad <= a2:
return # current approach orientation is within accepted range
else:
if a1 <= cad or cad <= a2:
return # current approach orientation is within accepted range
a1_diff = abs(M.normalize_deg(a1 - cad))
a2_diff = abs(M.normalize_deg(a2 - cad))
self.approach_orientation = a1 if a1_diff < a2_diff else a2 # fixed normalized orientation
def execute(self, reset, orientation, is_orientation_absolute, speed=1, stop=False):
'''
Parameters
----------
orientation : float
absolute or relative orientation of torso (relative to imu_torso_orientation), in degrees
set to None to dribble towards the opponent's goal (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
speed : float
speed from 0 to 1 (scale is not linear)
stop : bool
return True immediately if walking, wind down if dribbling, and return True when possible
'''
w = self.world
r = self.world.robot
me = r.loc_head_position[:2]
b = w.ball_abs_pos[:2]
b_rel = w.ball_rel_torso_cart_pos[:2]
b_dist = np.linalg.norm(b-me)
behavior = self.behavior
reset_dribble = False
lost_ball = (w.ball_last_seen <= w.time_local_ms - w.VISUALSTEP_MS) or np.linalg.norm(b_rel)>0.4
if reset:
self.phase = 0
if behavior.previous_behavior == "Push_RL" and 0<b_rel[0]<0.25 and abs(b_rel[1])<0.07:
self.phase = 1
reset_dribble = True
if self.phase == 0: # walk to ball
reset_walk = reset and behavior.previous_behavior != "Walk" and behavior.previous_behavior != "Push_RL" # reset walk if it wasn't the previous behavior
#------------------------ 1. Decide if a better approach orientation is needed (when ball is nearly out of bounds)
if reset or b_dist > 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<b_rel[0]<0.25 and abs(b_rel[1])<0.05 and w.ball_is_visible: # ready to start dribble
self.phase += 1
reset_dribble = True
else:
rel_target = b_rel+(-0.23,0) # relative target is a circle (center: ball, radius:0.23m)
rel_ori = M.vector_angle(b_rel) # relative orientation of ball, NOT the target!
dist = max(0.08, np.linalg.norm(rel_target)*0.7) # slow approach
behavior.execute_sub_behavior("Walk", reset_walk, rel_target, False, rel_ori, False, dist) # target, is_target_abs, ori, is_ori_abs, distance
if stop:
return True
if self.phase == 1 and (stop or (b_dist > 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

181
behaviors/custom/Dribble/Env.py
Unescape Escape View File

@ -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

BIN
behaviors/custom/Dribble/dribble_R0.pkl
View File

Binary file not shown.

BIN
behaviors/custom/Dribble/dribble_R1.pkl
View File

Binary file not shown.

BIN
behaviors/custom/Dribble/dribble_R2.pkl
View File

Binary file not shown.

BIN
behaviors/custom/Dribble/dribble_R3.pkl
View File

Binary file not shown.

BIN
behaviors/custom/Dribble/dribble_R4.pkl
View File

Binary file not shown.

43
behaviors/custom/Fall/Fall.py
Unescape Escape View File

@ -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

BIN
behaviors/custom/Fall/fall.pkl
View File

Binary file not shown.

68
behaviors/custom/Get_Up/Get_Up.py
Unescape Escape View File

@ -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)

75
behaviors/custom/Kick_8/Kick_8.py
Unescape Escape View File

@ -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

59
behaviors/custom/Step/Step.py
Unescape Escape View File

@ -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

75
behaviors/custom/Step/Step_Generator.py
Unescape Escape View File

@ -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

201
behaviors/custom/Walk/Env.py
Unescape Escape View File

@ -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

83
behaviors/custom/Walk/Walk.py
Unescape Escape View File

@ -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

BIN
behaviors/custom/Walk/walk_R0.pkl
View File

Binary file not shown.

BIN
behaviors/custom/Walk/walk_R1_R3.pkl
View File

Binary file not shown.

BIN
behaviors/custom/Walk/walk_R2.pkl
View File

Binary file not shown.

BIN
behaviors/custom/Walk/walk_R4.pkl
View File

Binary file not shown.

71
behaviors/slot/common/Dive_Left.xml
Unescape Escape View File

@ -0,0 +1,71 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Dive left (goalkeeper)" auto_head="1">
<slot delta="0.28">
<move id="2" angle="0"/>
<move id="3" angle="0"/>
<move id="4" angle="-25"/>
<move id="5" angle="45"/>
<move id="6" angle="60"/>
<move id="7" angle="0"/>
<move id="8" angle="-120"/>
<move id="9" angle="0"/>
<move id="10" angle="60"/>
<move id="11" angle="0"/>
<move id="12" angle="25"/>
<move id="13" angle="-45"/>
<move id="14" angle="90"/>
<move id="15" angle="90"/>
<move id="16" angle="0"/>
<move id="17" angle="0"/>
<move id="18" angle="90"/>
<move id="19" angle="90"/>
<move id="20" angle="0"/>
<move id="21" angle="0"/>
</slot>
<slot delta="0.18">
<move id="2" angle="0"/>
<move id="3" angle="0"/>
<move id="4" angle="-25"/>
<move id="5" angle="45"/>
<move id="6" angle="60"/>
<move id="7" angle="0"/>
<move id="8" angle="-120"/>
<move id="9" angle="0"/>
<move id="10" angle="60"/>
<move id="11" angle="0"/>
<move id="12" angle="25"/>
<move id="13" angle="-45"/>
<move id="14" angle="90"/>
<move id="15" angle="90"/>
<move id="16" angle="0"/>
<move id="17" angle="0"/>
<move id="18" angle="90"/>
<move id="19" angle="90"/>
<move id="20" angle="0"/>
<move id="21" angle="0"/>
</slot>
<slot delta="0.32">
<move id="2" angle="0"/>
<move id="3" angle="0"/>
<move id="4" angle="0"/>
<move id="5" angle="0"/>
<move id="6" angle="0"/>
<move id="7" angle="0"/>
<move id="8" angle="0"/>
<move id="9" angle="0"/>
<move id="10" angle="0"/>
<move id="11" angle="0"/>
<move id="12" angle="0"/>
<move id="13" angle="0"/>
<move id="14" angle="90"/>
<move id="15" angle="90"/>
<move id="16" angle="0"/>
<move id="17" angle="0"/>
<move id="18" angle="90"/>
<move id="19" angle="90"/>
<move id="20" angle="0"/>
<move id="21" angle="0"/>
</slot>
</behavior>

71
behaviors/slot/common/Dive_Right.xml
Unescape Escape View File

@ -0,0 +1,71 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Dive right (goalkeeper)" auto_head="1">
<slot delta="0.28">
<move id="2" angle="0"/>
<move id="3" angle="0"/>
<move id="4" angle="45"/>
<move id="5" angle="-25"/>
<move id="6" angle="0"/>
<move id="7" angle="60"/>
<move id="8" angle="0"/>
<move id="9" angle="-120"/>
<move id="10" angle="0"/>
<move id="11" angle="60"/>
<move id="12" angle="-45"/>
<move id="13" angle="25"/>
<move id="14" angle="90"/>
<move id="15" angle="90"/>
<move id="16" angle="0"/>
<move id="17" angle="0"/>
<move id="18" angle="90"/>
<move id="19" angle="90"/>
<move id="20" angle="0"/>
<move id="21" angle="0"/>
</slot>
<slot delta="0.18">
<move id="2" angle="0"/>
<move id="3" angle="0"/>
<move id="4" angle="45"/>
<move id="5" angle="-25"/>
<move id="6" angle="0"/>
<move id="7" angle="60"/>
<move id="8" angle="0"/>
<move id="9" angle="-120"/>
<move id="10" angle="0"/>
<move id="11" angle="60"/>
<move id="12" angle="-45"/>
<move id="13" angle="25"/>
<move id="14" angle="90"/>
<move id="15" angle="90"/>
<move id="16" angle="0"/>
<move id="17" angle="0"/>
<move id="18" angle="90"/>
<move id="19" angle="90"/>
<move id="20" angle="0"/>
<move id="21" angle="0"/>
</slot>
<slot delta="0.32">
<move id="2" angle="0"/>
<move id="3" angle="0"/>
<move id="4" angle="0"/>
<move id="5" angle="0"/>
<move id="6" angle="0"/>
<move id="7" angle="0"/>
<move id="8" angle="0"/>
<move id="9" angle="0"/>
<move id="10" angle="0"/>
<move id="11" angle="0"/>
<move id="12" angle="0"/>
<move id="13" angle="0"/>
<move id="14" angle="90"/>
<move id="15" angle="90"/>
<move id="16" angle="0"/>
<move id="17" angle="0"/>
<move id="18" angle="90"/>
<move id="19" angle="90"/>
<move id="20" angle="0"/>
<move id="21" angle="0"/>
</slot>
</behavior>

12
behaviors/slot/common/Flip.xml
Unescape Escape View File

@ -0,0 +1,12 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Flip from sides before getting up" auto_head="0">
<slot delta="0.30">
<move id="14" angle="-120"/>
<move id="15" angle="-120"/>
</slot>
<slot delta="0.30">
<move id="16" angle="80"/>
<move id="17" angle="80"/>
</slot>
</behavior>

24
behaviors/slot/common/Squat.xml
Unescape Escape View File

@ -0,0 +1,24 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Squat for demonstrations" auto_head="0">
<slot delta="1.0"> <!-- GO DOWN -->
<move id="6" angle="40"/> <!-- Left leg pitch -->
<move id="7" angle="40"/> <!-- Right leg pitch -->
<move id="8" angle="-80"/> <!-- Left knee -->
<move id="9" angle="-80"/> <!-- Right knee -->
<move id="10" angle="40"/> <!-- Left foot pitch -->
<move id="11" angle="40"/> <!-- Right foot pitch -->
<move id="16" angle="30"/> <!-- Left arm roll -->
<move id="17" angle="30"/> <!-- Right arm roll -->
</slot>
<slot delta="1.0"> <!-- GO UP -->
<move id="6" angle="0"/> <!-- Left leg pitch -->
<move id="7" angle="0"/> <!-- Right leg pitch -->
<move id="8" angle="0"/> <!-- Left knee -->
<move id="9" angle="0"/> <!-- Right knee -->
<move id="10" angle="0"/> <!-- Left foot pitch -->
<move id="11" angle="0"/> <!-- Right foot pitch -->
<move id="16" angle="0"/> <!-- Left arm roll -->
<move id="17" angle="0"/> <!-- Right arm roll -->
</slot>
</behavior>

148
behaviors/slot/r0/Get_Up_Back.xml
Unescape Escape View File

@ -0,0 +1,148 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Get up from robot's back starting from Zero pose" auto_head="0">
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-35.454876"/>
<move id="3" angle="-35.454876"/>
<move id="4" angle="9.4003105"/>
<move id="5" angle="9.4003105"/>
<move id="6" angle="106.73567"/>
<move id="7" angle="106.73567"/>
<move id="8" angle="23.08675"/>
<move id="9" angle="23.08675"/>
<move id="10" angle="-19.941034"/>
<move id="11" angle="-19.941034"/>
<move id="12" angle="8.216015"/>
<move id="13" angle="8.216015"/>
<move id="14" angle="57.021896"/>
<move id="15" angle="57.021896"/>
<move id="16" angle="-64.761406"/>
<move id="17" angle="-64.761406"/>
<move id="18" angle="-120.64462"/>
<move id="19" angle="-120.64462"/>
<move id="20" angle="85.76495"/>
<move id="21" angle="85.76495"/>
</slot>
<slot delta="0.23555405990408101">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-62.22426"/>
<move id="3" angle="-62.22426"/>
<move id="4" angle="-11.9137745"/>
<move id="5" angle="-11.9137745"/>
<move id="6" angle="24.511059"/>
<move id="7" angle="24.511059"/>
<move id="8" angle="14.4840355"/>
<move id="9" angle="14.4840355"/>
<move id="10" angle="53.158936"/>
<move id="11" angle="53.158936"/>
<move id="12" angle="-14.684006"/>
<move id="13" angle="-14.684006"/>
<move id="14" angle="-7.403747"/>
<move id="15" angle="-7.403747"/>
<move id="16" angle="60.993023"/>
<move id="17" angle="60.993023"/>
<move id="18" angle="106.169014"/>
<move id="19" angle="106.169014"/>
<move id="20" angle="47.9693"/>
<move id="21" angle="47.9693"/>
</slot>
<slot delta="0.32551713739126387">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-47.479836"/>
<move id="3" angle="-47.479836"/>
<move id="4" angle="24.034796"/>
<move id="5" angle="24.034796"/>
<move id="6" angle="143.53075"/>
<move id="7" angle="143.53075"/>
<move id="8" angle="-44.24652"/>
<move id="9" angle="-44.24652"/>
<move id="10" angle="-65.46661"/>
<move id="11" angle="-65.46661"/>
<move id="12" angle="-34.403675"/>
<move id="13" angle="-34.403675"/>
<move id="14" angle="63.59447"/>
<move id="15" angle="63.59447"/>
<move id="16" angle="-49.916084"/>
<move id="17" angle="-49.916084"/>
<move id="18" angle="-98.035194"/>
<move id="19" angle="-98.035194"/>
<move id="20" angle="-17.168068"/>
<move id="21" angle="-17.168068"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-66.561325"/>
<move id="3" angle="-66.561325"/>
<move id="4" angle="37.05487"/>
<move id="5" angle="37.05487"/>
<move id="6" angle="29.69468"/>
<move id="7" angle="29.69468"/>
<move id="8" angle="-54.39078"/>
<move id="9" angle="-54.39078"/>
<move id="10" angle="-60.952106"/>
<move id="11" angle="-60.952106"/>
<move id="12" angle="-29.30997"/>
<move id="13" angle="-29.30997"/>
<move id="14" angle="46.348843"/>
<move id="15" angle="46.348843"/>
<move id="16" angle="127.48256"/>
<move id="17" angle="127.48256"/>
<move id="18" angle="126.34747"/>
<move id="19" angle="126.34747"/>
<move id="20" angle="32.387558"/>
<move id="21" angle="32.387558"/>
</slot>
<slot delta="0.04259131836792499">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-20.94111"/>
<move id="3" angle="-20.94111"/>
<move id="4" angle="45.91111"/>
<move id="5" angle="45.91111"/>
<move id="6" angle="112.662"/>
<move id="7" angle="112.662"/>
<move id="8" angle="-134.07819"/>
<move id="9" angle="-134.07819"/>
<move id="10" angle="39.1108"/>
<move id="11" angle="39.1108"/>
<move id="12" angle="-41.35012"/>
<move id="13" angle="-41.35012"/>
<move id="14" angle="-27.910278"/>
<move id="15" angle="-27.910278"/>
<move id="16" angle="16.615406"/>
<move id="17" angle="16.615406"/>
<move id="18" angle="-107.39476"/>
<move id="19" angle="-107.39476"/>
<move id="20" angle="75.266754"/>
<move id="21" angle="75.266754"/>
</slot>
<slot delta="0.14964306950470477">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="2.0762901"/>
<move id="3" angle="2.0762901"/>
<move id="4" angle="53.06344"/>
<move id="5" angle="53.06344"/>
<move id="6" angle="138.88753"/>
<move id="7" angle="138.88753"/>
<move id="8" angle="-131.63484"/>
<move id="9" angle="-131.63484"/>
<move id="10" angle="31.228016"/>
<move id="11" angle="31.228016"/>
<move id="12" angle="-13.720424"/>
<move id="13" angle="-13.720424"/>
<move id="14" angle="43.981934"/>
<move id="15" angle="43.981934"/>
<move id="16" angle="-0.4473343"/>
<move id="17" angle="-0.4473343"/>
<move id="18" angle="13.684681"/>
<move id="19" angle="13.684681"/>
<move id="20" angle="-15.960043"/>
<move id="21" angle="-15.960043"/>
</slot>
</behavior>

220
behaviors/slot/r0/Get_Up_Front.xml
Unescape Escape View File

@ -0,0 +1,220 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Get up from robot's front starting from Zero pose" auto_head="0">
<slot delta="0.2">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-4.976922"/>
<move id="3" angle="-4.976922"/>
<move id="4" angle="14.641327"/>
<move id="5" angle="14.641327"/>
<move id="6" angle="6.7912245"/>
<move id="7" angle="6.7912245"/>
<move id="8" angle="-0.386917"/>
<move id="9" angle="-0.386917"/>
<move id="10" angle="0.23039937"/>
<move id="11" angle="0.23039937"/>
<move id="12" angle="-5.228052"/>
<move id="13" angle="-5.228052"/>
<move id="14" angle="23.919197"/>
<move id="15" angle="23.919197"/>
<move id="16" angle="9.847563"/>
<move id="17" angle="9.847563"/>
<move id="18" angle="-6.9379196"/>
<move id="19" angle="-6.9379196"/>
<move id="20" angle="-16.700315"/>
<move id="21" angle="-16.700315"/>
</slot>
<slot delta="0.08">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="1.0232245"/>
<move id="3" angle="1.0232245"/>
<move id="4" angle="-22.36497"/>
<move id="5" angle="-22.36497"/>
<move id="6" angle="265.17767"/>
<move id="7" angle="265.17767"/>
<move id="8" angle="-133.80704"/>
<move id="9" angle="-133.80704"/>
<move id="10" angle="118.27272"/>
<move id="11" angle="118.27272"/>
<move id="12" angle="-7.289217"/>
<move id="13" angle="-7.289217"/>
<move id="14" angle="2.7956364"/>
<move id="15" angle="2.7956364"/>
<move id="16" angle="17.118933"/>
<move id="17" angle="17.118933"/>
<move id="18" angle="-27.083485"/>
<move id="19" angle="-27.083485"/>
<move id="20" angle="-8.698996"/>
<move id="21" angle="-8.698996"/>
</slot>
<slot delta="0.08">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-14.808299"/>
<move id="3" angle="-14.808299"/>
<move id="4" angle="37.39434"/>
<move id="5" angle="37.39434"/>
<move id="6" angle="7.6876955"/>
<move id="7" angle="7.6876955"/>
<move id="8" angle="-91.4461"/>
<move id="9" angle="-91.4461"/>
<move id="10" angle="-0.40869182"/>
<move id="11" angle="-0.40869182"/>
<move id="12" angle="-24.007051"/>
<move id="13" angle="-24.007051"/>
<move id="14" angle="8.485344"/>
<move id="15" angle="8.485344"/>
<move id="16" angle="7.828232"/>
<move id="17" angle="7.828232"/>
<move id="18" angle="5.688171"/>
<move id="19" angle="5.688171"/>
<move id="20" angle="-17.660221"/>
<move id="21" angle="-17.660221"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-89.41741"/>
<move id="3" angle="-89.41741"/>
<move id="4" angle="14.217276"/>
<move id="5" angle="14.217276"/>
<move id="6" angle="-3.3899987"/>
<move id="7" angle="-3.3899987"/>
<move id="8" angle="-103.90058"/>
<move id="9" angle="-103.90058"/>
<move id="10" angle="-48.087368"/>
<move id="11" angle="-48.087368"/>
<move id="12" angle="-21.949076"/>
<move id="13" angle="-21.949076"/>
<move id="14" angle="-0.5262981"/>
<move id="15" angle="-0.5262981"/>
<move id="16" angle="-9.913716"/>
<move id="17" angle="-9.913716"/>
<move id="18" angle="-0.202469"/>
<move id="19" angle="-0.202469"/>
<move id="20" angle="-12.194956"/>
<move id="21" angle="-12.194956"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-0.9640895"/>
<move id="3" angle="-0.9640895"/>
<move id="4" angle="-4.2894506"/>
<move id="5" angle="-4.2894506"/>
<move id="6" angle="-1.5769005"/>
<move id="7" angle="-1.5769005"/>
<move id="8" angle="5.6831093"/>
<move id="9" angle="5.6831093"/>
<move id="10" angle="4.317226"/>
<move id="11" angle="4.317226"/>
<move id="12" angle="-3.2686572"/>
<move id="13" angle="-3.2686572"/>
<move id="14" angle="50.114185"/>
<move id="15" angle="50.114185"/>
<move id="16" angle="-11.285558"/>
<move id="17" angle="-11.285558"/>
<move id="18" angle="-6.0098124"/>
<move id="19" angle="-6.0098124"/>
<move id="20" angle="-3.112287"/>
<move id="21" angle="-3.112287"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-60.664845"/>
<move id="3" angle="-60.664845"/>
<move id="4" angle="15.481741"/>
<move id="5" angle="15.481741"/>
<move id="6" angle="-1.4592165"/>
<move id="7" angle="-1.4592165"/>
<move id="8" angle="-121.534065"/>
<move id="9" angle="-121.534065"/>
<move id="10" angle="44.071327"/>
<move id="11" angle="44.071327"/>
<move id="12" angle="-10.756018"/>
<move id="13" angle="-10.756018"/>
<move id="14" angle="-4.711259"/>
<move id="15" angle="-4.711259"/>
<move id="16" angle="-7.3120046"/>
<move id="17" angle="-7.3120046"/>
<move id="18" angle="3.982705"/>
<move id="19" angle="3.982705"/>
<move id="20" angle="-11.348281"/>
<move id="21" angle="-11.348281"/>
</slot>
<slot delta="0.04">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-58.631565"/>
<move id="3" angle="-58.631565"/>
<move id="4" angle="31.81041"/>
<move id="5" angle="31.81041"/>
<move id="6" angle="100.52844"/>
<move id="7" angle="100.52844"/>
<move id="8" angle="-123.12153"/>
<move id="9" angle="-123.12153"/>
<move id="10" angle="38.84296"/>
<move id="11" angle="38.84296"/>
<move id="12" angle="-37.32096"/>
<move id="13" angle="-37.32096"/>
<move id="14" angle="-4.5341907"/>
<move id="15" angle="-4.5341907"/>
<move id="16" angle="6.23005"/>
<move id="17" angle="6.23005"/>
<move id="18" angle="12.395767"/>
<move id="19" angle="12.395767"/>
<move id="20" angle="-4.4955177"/>
<move id="21" angle="-4.4955177"/>
</slot>
<slot delta="0.14">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-62.06845"/>
<move id="3" angle="-62.06845"/>
<move id="4" angle="69.89282"/>
<move id="5" angle="69.89282"/>
<move id="6" angle="104.76725"/>
<move id="7" angle="104.76725"/>
<move id="8" angle="-132.93317"/>
<move id="9" angle="-132.93317"/>
<move id="10" angle="35.730618"/>
<move id="11" angle="35.730618"/>
<move id="12" angle="-33.17914"/>
<move id="13" angle="-33.17914"/>
<move id="14" angle="-19.49921"/>
<move id="15" angle="-19.49921"/>
<move id="16" angle="0.18734631"/>
<move id="17" angle="0.18734631"/>
<move id="18" angle="20.604797"/>
<move id="19" angle="20.604797"/>
<move id="20" angle="8.590274"/>
<move id="21" angle="8.590274"/>
</slot>
<slot delta="0.4">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-0.702901"/>
<move id="3" angle="-0.702901"/>
<move id="4" angle="7.3007526"/>
<move id="5" angle="7.3007526"/>
<move id="6" angle="36.762917"/>
<move id="7" angle="36.762917"/>
<move id="8" angle="-102.52497"/>
<move id="9" angle="-102.52497"/>
<move id="10" angle="65.24182"/>
<move id="11" angle="65.24182"/>
<move id="12" angle="7.8809857"/>
<move id="13" angle="7.8809857"/>
<move id="14" angle="-117.089615"/>
<move id="15" angle="-117.089615"/>
<move id="16" angle="37.752632"/>
<move id="17" angle="37.752632"/>
<move id="18" angle="47.93907"/>
<move id="19" angle="47.93907"/>
<move id="20" angle="-8.804468"/>
<move id="21" angle="-8.804468"/>
</slot>
</behavior>

21
behaviors/slot/r0/Kick_Motion.xml
Unescape Escape View File

@ -0,0 +1,21 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Kick motion with right leg" auto_head="1">
<slot delta="0.22"> <!-- Lean -->
<move id="5" angle="-10"/> <!-- Left leg roll -->
<move id="6" angle="40"/> <!-- Left leg pitch -->
<move id="7" angle="65"/> <!-- Right leg pitch -->
<move id="8" angle="-60"/> <!-- Left knee -->
<move id="9" angle="-115"/> <!-- Right knee -->
<move id="10" angle="60"/> <!-- Left foot pitch -->
<move id="11" angle="10"/> <!-- Right foot pitch -->
</slot>
<slot delta="0.12"> <!-- Kick -->
<move id="3" angle="-45"/> <!-- Right leg yaw/pitch -->
<move id="6" angle="-25"/> <!-- Left leg pitch -->
<move id="7" angle="80"/> <!-- Right leg pitch -->
<move id="8" angle="0"/> <!-- Left knee -->
<move id="9" angle="0"/> <!-- Right knee -->
<move id="10" angle="30"/> <!-- Left foot pitch -->
</slot>
</behavior>

148
behaviors/slot/r1/Get_Up_Back.xml
Unescape Escape View File

@ -0,0 +1,148 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Get up from robot's back starting from Zero pose" auto_head="0">
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-33.468"/>
<move id="3" angle="-33.468"/>
<move id="4" angle="32.57773"/>
<move id="5" angle="32.57773"/>
<move id="6" angle="59.169636"/>
<move id="7" angle="59.169636"/>
<move id="8" angle="9.965677"/>
<move id="9" angle="9.965677"/>
<move id="10" angle="-2.0611076"/>
<move id="11" angle="-2.0611076"/>
<move id="12" angle="0.4622302"/>
<move id="13" angle="0.4622302"/>
<move id="14" angle="59.513363"/>
<move id="15" angle="59.513363"/>
<move id="16" angle="-65.481674"/>
<move id="17" angle="-65.481674"/>
<move id="18" angle="-40.493507"/>
<move id="19" angle="-40.493507"/>
<move id="20" angle="173.76538"/>
<move id="21" angle="173.76538"/>
</slot>
<slot delta="0.24">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-53.970947"/>
<move id="3" angle="-53.970947"/>
<move id="4" angle="-31.631035"/>
<move id="5" angle="-31.631035"/>
<move id="6" angle="19.487873"/>
<move id="7" angle="19.487873"/>
<move id="8" angle="16.130121"/>
<move id="9" angle="16.130121"/>
<move id="10" angle="48.766438"/>
<move id="11" angle="48.766438"/>
<move id="12" angle="-43.63519"/>
<move id="13" angle="-43.63519"/>
<move id="14" angle="8.0750675"/>
<move id="15" angle="8.0750675"/>
<move id="16" angle="26.09803"/>
<move id="17" angle="26.09803"/>
<move id="18" angle="76.53693"/>
<move id="19" angle="76.53693"/>
<move id="20" angle="166.2738"/>
<move id="21" angle="166.2738"/>
</slot>
<slot delta="0.32">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-58.35547"/>
<move id="3" angle="-58.35547"/>
<move id="4" angle="4.337753"/>
<move id="5" angle="4.337753"/>
<move id="6" angle="119.925354"/>
<move id="7" angle="119.925354"/>
<move id="8" angle="-24.74408"/>
<move id="9" angle="-24.74408"/>
<move id="10" angle="-96.68003"/>
<move id="11" angle="-96.68003"/>
<move id="12" angle="-38.341896"/>
<move id="13" angle="-38.341896"/>
<move id="14" angle="61.39873"/>
<move id="15" angle="61.39873"/>
<move id="16" angle="-69.773056"/>
<move id="17" angle="-69.773056"/>
<move id="18" angle="-76.832436"/>
<move id="19" angle="-76.832436"/>
<move id="20" angle="-33.894478"/>
<move id="21" angle="-33.894478"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-82.921234"/>
<move id="3" angle="-82.921234"/>
<move id="4" angle="62.38533"/>
<move id="5" angle="62.38533"/>
<move id="6" angle="-25.176954"/>
<move id="7" angle="-25.176954"/>
<move id="8" angle="-48.75859"/>
<move id="9" angle="-48.75859"/>
<move id="10" angle="-65.601974"/>
<move id="11" angle="-65.601974"/>
<move id="12" angle="-22.979483"/>
<move id="13" angle="-22.979483"/>
<move id="14" angle="5.9137707"/>
<move id="15" angle="5.9137707"/>
<move id="16" angle="21.222649"/>
<move id="17" angle="21.222649"/>
<move id="18" angle="-84.464584"/>
<move id="19" angle="-84.464584"/>
<move id="20" angle="38.63583"/>
<move id="21" angle="38.63583"/>
</slot>
<slot delta="0.06">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-28.479536"/>
<move id="3" angle="-28.479536"/>
<move id="4" angle="63.585136"/>
<move id="5" angle="63.585136"/>
<move id="6" angle="111.310234"/>
<move id="7" angle="111.310234"/>
<move id="8" angle="-159.30522"/>
<move id="9" angle="-159.30522"/>
<move id="10" angle="56.21947"/>
<move id="11" angle="56.21947"/>
<move id="12" angle="-46.503235"/>
<move id="13" angle="-46.503235"/>
<move id="14" angle="11.373715"/>
<move id="15" angle="11.373715"/>
<move id="16" angle="-54.145515"/>
<move id="17" angle="-54.145515"/>
<move id="18" angle="63.853237"/>
<move id="19" angle="63.853237"/>
<move id="20" angle="-32.70381"/>
<move id="21" angle="-32.70381"/>
</slot>
<slot delta="0.16">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-3.9309306"/>
<move id="3" angle="-3.9309306"/>
<move id="4" angle="28.753582"/>
<move id="5" angle="28.753582"/>
<move id="6" angle="73.12465"/>
<move id="7" angle="73.12465"/>
<move id="8" angle="-133.79837"/>
<move id="9" angle="-133.79837"/>
<move id="10" angle="37.535084"/>
<move id="11" angle="37.535084"/>
<move id="12" angle="5.768486"/>
<move id="13" angle="5.768486"/>
<move id="14" angle="-69.01422"/>
<move id="15" angle="-69.01422"/>
<move id="16" angle="50.282738"/>
<move id="17" angle="50.282738"/>
<move id="18" angle="51.603962"/>
<move id="19" angle="51.603962"/>
<move id="20" angle="80.4043"/>
<move id="21" angle="80.4043"/>
</slot>
</behavior>

220
behaviors/slot/r1/Get_Up_Front.xml
Unescape Escape View File

@ -0,0 +1,220 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Get up from robot's front starting from Zero pose" auto_head="0">
<slot delta="0.04">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="0.03765949"/>
<move id="3" angle="0.03765949"/>
<move id="4" angle="6.7361"/>
<move id="5" angle="6.7361"/>
<move id="6" angle="7.144582"/>
<move id="7" angle="7.144582"/>
<move id="8" angle="3.2917228"/>
<move id="9" angle="3.2917228"/>
<move id="10" angle="0.2270207"/>
<move id="11" angle="0.2270207"/>
<move id="12" angle="-0.62398136"/>
<move id="13" angle="-0.62398136"/>
<move id="14" angle="11.943922"/>
<move id="15" angle="11.943922"/>
<move id="16" angle="36.128757"/>
<move id="17" angle="36.128757"/>
<move id="18" angle="1.7096568"/>
<move id="19" angle="1.7096568"/>
<move id="20" angle="-10.422138"/>
<move id="21" angle="-10.422138"/>
</slot>
<slot delta="0.18">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="1.2124634"/>
<move id="3" angle="1.2124634"/>
<move id="4" angle="-16.6976"/>
<move id="5" angle="-16.6976"/>
<move id="6" angle="12.002005"/>
<move id="7" angle="12.002005"/>
<move id="8" angle="-3.8902621"/>
<move id="9" angle="-3.8902621"/>
<move id="10" angle="2.2115564"/>
<move id="11" angle="2.2115564"/>
<move id="12" angle="18.24385"/>
<move id="13" angle="18.24385"/>
<move id="14" angle="17.7995"/>
<move id="15" angle="17.7995"/>
<move id="16" angle="46.32759"/>
<move id="17" angle="46.32759"/>
<move id="18" angle="-5.752133"/>
<move id="19" angle="-5.752133"/>
<move id="20" angle="-23.737434"/>
<move id="21" angle="-23.737434"/>
</slot>
<slot delta="0.06">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="0.4612013"/>
<move id="3" angle="0.4612013"/>
<move id="4" angle="-10.992544"/>
<move id="5" angle="-10.992544"/>
<move id="6" angle="256.44278"/>
<move id="7" angle="256.44278"/>
<move id="8" angle="-130.99982"/>
<move id="9" angle="-130.99982"/>
<move id="10" angle="125.019295"/>
<move id="11" angle="125.019295"/>
<move id="12" angle="-4.830559"/>
<move id="13" angle="-4.830559"/>
<move id="14" angle="-3.4743867"/>
<move id="15" angle="-3.4743867"/>
<move id="16" angle="-17.395947"/>
<move id="17" angle="-17.395947"/>
<move id="18" angle="-7.197071"/>
<move id="19" angle="-7.197071"/>
<move id="20" angle="-18.81015"/>
<move id="21" angle="-18.81015"/>
</slot>
<slot delta="0.06">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-11.333593"/>
<move id="3" angle="-11.333593"/>
<move id="4" angle="44.738525"/>
<move id="5" angle="44.738525"/>
<move id="6" angle="6.1589823"/>
<move id="7" angle="6.1589823"/>
<move id="8" angle="-91.90975"/>
<move id="9" angle="-91.90975"/>
<move id="10" angle="3.823912"/>
<move id="11" angle="3.823912"/>
<move id="12" angle="-38.074776"/>
<move id="13" angle="-38.074776"/>
<move id="14" angle="15.952488"/>
<move id="15" angle="15.952488"/>
<move id="16" angle="-7.0655346"/>
<move id="17" angle="-7.0655346"/>
<move id="18" angle="-22.311245"/>
<move id="19" angle="-22.311245"/>
<move id="20" angle="-18.258038"/>
<move id="21" angle="-18.258038"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-89.49788"/>
<move id="3" angle="-89.49788"/>
<move id="4" angle="-3.2911317"/>
<move id="5" angle="-3.2911317"/>
<move id="6" angle="-2.9935064"/>
<move id="7" angle="-2.9935064"/>
<move id="8" angle="-97.67326"/>
<move id="9" angle="-97.67326"/>
<move id="10" angle="-45.384357"/>
<move id="11" angle="-45.384357"/>
<move id="12" angle="-23.928814"/>
<move id="13" angle="-23.928814"/>
<move id="14" angle="-8.738391"/>
<move id="15" angle="-8.738391"/>
<move id="16" angle="-5.594632"/>
<move id="17" angle="-5.594632"/>
<move id="18" angle="12.174933"/>
<move id="19" angle="12.174933"/>
<move id="20" angle="9.707994"/>
<move id="21" angle="9.707994"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-3.2422483"/>
<move id="3" angle="-3.2422483"/>
<move id="4" angle="10.094491"/>
<move id="5" angle="10.094491"/>
<move id="6" angle="0.105650306"/>
<move id="7" angle="0.105650306"/>
<move id="8" angle="3.6602314"/>
<move id="9" angle="3.6602314"/>
<move id="10" angle="4.2006454"/>
<move id="11" angle="4.2006454"/>
<move id="12" angle="12.860165"/>
<move id="13" angle="12.860165"/>
<move id="14" angle="41.729637"/>
<move id="15" angle="41.729637"/>
<move id="16" angle="4.5020647"/>
<move id="17" angle="4.5020647"/>
<move id="18" angle="0.37317705"/>
<move id="19" angle="0.37317705"/>
<move id="20" angle="-17.251738"/>
<move id="21" angle="-17.251738"/>
</slot>
<slot delta="0.04">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-54.546894"/>
<move id="3" angle="-54.546894"/>
<move id="4" angle="47.746372"/>
<move id="5" angle="47.746372"/>
<move id="6" angle="102.24254"/>
<move id="7" angle="102.24254"/>
<move id="8" angle="-124.605934"/>
<move id="9" angle="-124.605934"/>
<move id="10" angle="48.503883"/>
<move id="11" angle="48.503883"/>
<move id="12" angle="-61.207714"/>
<move id="13" angle="-61.207714"/>
<move id="14" angle="3.2614079"/>
<move id="15" angle="3.2614079"/>
<move id="16" angle="14.854502"/>
<move id="17" angle="14.854502"/>
<move id="18" angle="-23.213947"/>
<move id="19" angle="-23.213947"/>
<move id="20" angle="6.2566423"/>
<move id="21" angle="6.2566423"/>
</slot>
<slot delta="0.14">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-49.910076"/>
<move id="3" angle="-49.910076"/>
<move id="4" angle="41.691715"/>
<move id="5" angle="41.691715"/>
<move id="6" angle="115.67641"/>
<move id="7" angle="115.67641"/>
<move id="8" angle="-130.50008"/>
<move id="9" angle="-130.50008"/>
<move id="10" angle="45.15649"/>
<move id="11" angle="45.15649"/>
<move id="12" angle="-63.601254"/>
<move id="13" angle="-63.601254"/>
<move id="14" angle="-23.61024"/>
<move id="15" angle="-23.61024"/>
<move id="16" angle="-22.336332"/>
<move id="17" angle="-22.336332"/>
<move id="18" angle="9.258426"/>
<move id="19" angle="9.258426"/>
<move id="20" angle="75"/> <!-- avoid self collision -->
<move id="21" angle="75"/> <!-- avoid self collision -->
</slot>
<slot delta="0.38">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-1.9032228"/>
<move id="3" angle="-1.9032228"/>
<move id="4" angle="25.761885"/>
<move id="5" angle="25.761885"/>
<move id="6" angle="37.294437"/>
<move id="7" angle="37.294437"/>
<move id="8" angle="-92.91941"/>
<move id="9" angle="-92.91941"/>
<move id="10" angle="54.069553"/>
<move id="11" angle="54.069553"/>
<move id="12" angle="3.6394153"/>
<move id="13" angle="3.6394153"/>
<move id="14" angle="-115.9809"/>
<move id="15" angle="-115.9809"/>
<move id="16" angle="30.516266"/>
<move id="17" angle="30.516266"/>
<move id="18" angle="67.11856"/>
<move id="19" angle="67.11856"/>
<move id="20" angle="75"/> <!-- avoid self collision -->
<move id="21" angle="75"/> <!-- avoid self collision -->
</slot>
</behavior>

21
behaviors/slot/r1/Kick_Motion.xml
Unescape Escape View File

@ -0,0 +1,21 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Kick motion with right leg" auto_head="1">
<slot delta="0.22"> <!-- Lean -->
<move id="5" angle="-10"/> <!-- Left leg roll -->
<move id="6" angle="40"/> <!-- Left leg pitch -->
<move id="7" angle="65"/> <!-- Right leg pitch -->
<move id="8" angle="-60"/> <!-- Left knee -->
<move id="9" angle="-115"/> <!-- Right knee -->
<move id="10" angle="60"/> <!-- Left foot pitch -->
<move id="11" angle="0"/> <!-- Right foot pitch -->
</slot>
<slot delta="0.12"> <!-- Kick -->
<move id="3" angle="-45"/> <!-- Right leg yaw/pitch -->
<move id="6" angle="-25"/> <!-- Left leg pitch -->
<move id="7" angle="80"/> <!-- Right leg pitch -->
<move id="8" angle="0"/> <!-- Left knee -->
<move id="9" angle="0"/> <!-- Right knee -->
<move id="10" angle="30"/> <!-- Left foot pitch -->
</slot>
</behavior>

148
behaviors/slot/r2/Get_Up_Back.xml
Unescape Escape View File

@ -0,0 +1,148 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Get up from robot's back starting from Zero pose" auto_head="0">
<slot delta="0.028081112120805277">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-24.101269"/>
<move id="3" angle="-24.101269"/>
<move id="4" angle="32.993332"/>
<move id="5" angle="32.993332"/>
<move id="6" angle="110.35986"/>
<move id="7" angle="110.35986"/>
<move id="8" angle="1.8788892"/>
<move id="9" angle="1.8788892"/>
<move id="10" angle="80.01187"/>
<move id="11" angle="80.01187"/>
<move id="12" angle="9.717135"/>
<move id="13" angle="9.717135"/>
<move id="14" angle="51.12714"/>
<move id="15" angle="51.12714"/>
<move id="16" angle="42.79977"/>
<move id="17" angle="42.79977"/>
<move id="18" angle="-86.264915"/>
<move id="19" angle="-86.264915"/>
<move id="20" angle="33.84708"/>
<move id="21" angle="33.84708"/>
</slot>
<slot delta="0.23351845363901297">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-45.0749"/>
<move id="3" angle="-45.0749"/>
<move id="4" angle="-14.139231"/>
<move id="5" angle="-14.139231"/>
<move id="6" angle="45.828407"/>
<move id="7" angle="45.828407"/>
<move id="8" angle="12.7746315"/>
<move id="9" angle="12.7746315"/>
<move id="10" angle="39.730194"/>
<move id="11" angle="39.730194"/>
<move id="12" angle="-41.445404"/>
<move id="13" angle="-41.445404"/>
<move id="14" angle="-20.96023"/>
<move id="15" angle="-20.96023"/>
<move id="16" angle="67.418335"/>
<move id="17" angle="67.418335"/>
<move id="18" angle="136.93271"/>
<move id="19" angle="136.93271"/>
<move id="20" angle="79.51946"/>
<move id="21" angle="79.51946"/>
</slot>
<slot delta="0.3334680044481105">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-47.639645"/>
<move id="3" angle="-47.639645"/>
<move id="4" angle="36.16746"/>
<move id="5" angle="36.16746"/>
<move id="6" angle="106.46471"/>
<move id="7" angle="106.46471"/>
<move id="8" angle="-39.363796"/>
<move id="9" angle="-39.363796"/>
<move id="10" angle="-72.647545"/>
<move id="11" angle="-72.647545"/>
<move id="12" angle="-25.91456"/>
<move id="13" angle="-25.91456"/>
<move id="14" angle="72.665886"/>
<move id="15" angle="72.665886"/>
<move id="16" angle="-46.327827"/>
<move id="17" angle="-46.327827"/>
<move id="18" angle="-66.61269"/>
<move id="19" angle="-66.61269"/>
<move id="20" angle="-31.427937"/>
<move id="21" angle="-31.427937"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-75.75934"/>
<move id="3" angle="-75.75934"/>
<move id="4" angle="22.589397"/>
<move id="5" angle="22.589397"/>
<move id="6" angle="29.006308"/>
<move id="7" angle="29.006308"/>
<move id="8" angle="-38.235386"/>
<move id="9" angle="-38.235386"/>
<move id="10" angle="-60.07112"/>
<move id="11" angle="-60.07112"/>
<move id="12" angle="-7.1391"/>
<move id="13" angle="-7.1391"/>
<move id="14" angle="-4.2825775"/>
<move id="15" angle="-4.2825775"/>
<move id="16" angle="134.62912"/>
<move id="17" angle="134.62912"/>
<move id="18" angle="131.15475"/>
<move id="19" angle="131.15475"/>
<move id="20" angle="73.77145"/>
<move id="21" angle="73.77145"/>
</slot>
<slot delta="0.05570955407520801">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-19.975779"/>
<move id="3" angle="-19.975779"/>
<move id="4" angle="41.732513"/>
<move id="5" angle="41.732513"/>
<move id="6" angle="102.82768"/>
<move id="7" angle="102.82768"/>
<move id="8" angle="-136.81155"/>
<move id="9" angle="-136.81155"/>
<move id="10" angle="14.452017"/>
<move id="11" angle="14.452017"/>
<move id="12" angle="-36.594322"/>
<move id="13" angle="-36.594322"/>
<move id="14" angle="-10.283749"/>
<move id="15" angle="-10.283749"/>
<move id="16" angle="46.129036"/>
<move id="17" angle="46.129036"/>
<move id="18" angle="-29.25866"/>
<move id="19" angle="-29.25866"/>
<move id="20" angle="-28.004427"/>
<move id="21" angle="-28.004427"/>
</slot>
<slot delta="0.15631983125111132">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-1.2392063"/>
<move id="3" angle="-1.2392063"/>
<move id="4" angle="50.825813"/>
<move id="5" angle="50.825813"/>
<move id="6" angle="81.91005"/>
<move id="7" angle="81.91005"/>
<move id="8" angle="-114.5607"/>
<move id="9" angle="-114.5607"/>
<move id="10" angle="46.413334"/>
<move id="11" angle="46.413334"/>
<move id="12" angle="-11.515432"/>
<move id="13" angle="-11.515432"/>
<move id="14" angle="-17.704185"/>
<move id="15" angle="-17.704185"/>
<move id="16" angle="11.881702"/>
<move id="17" angle="11.881702"/>
<move id="18" angle="116.02219"/>
<move id="19" angle="116.02219"/>
<move id="20" angle="85.50837"/>
<move id="21" angle="85.50837"/>
</slot>
</behavior>

220
behaviors/slot/r2/Get_Up_Front.xml
Unescape Escape View File

@ -0,0 +1,220 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Get up from robot's front starting from Zero pose" auto_head="0">
<slot delta="0.16">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="4.6313915"/>
<move id="3" angle="4.6313915"/>
<move id="4" angle="-11.846246"/>
<move id="5" angle="-11.846246"/>
<move id="6" angle="8.745557"/>
<move id="7" angle="8.745557"/>
<move id="8" angle="-1.8197118"/>
<move id="9" angle="-1.8197118"/>
<move id="10" angle="1.359597"/>
<move id="11" angle="1.359597"/>
<move id="12" angle="16.82137"/>
<move id="13" angle="16.82137"/>
<move id="14" angle="21.83775"/>
<move id="15" angle="21.83775"/>
<move id="16" angle="-27.174564"/>
<move id="17" angle="-27.174564"/>
<move id="18" angle="-12.889872"/>
<move id="19" angle="-12.889872"/>
<move id="20" angle="-14.989797"/>
<move id="21" angle="-14.989797"/>
</slot>
<slot delta="0.08">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="1.0406766"/>
<move id="3" angle="1.0406766"/>
<move id="4" angle="-7.939904"/>
<move id="5" angle="-7.939904"/>
<move id="6" angle="260.59564"/>
<move id="7" angle="260.59564"/>
<move id="8" angle="-131.98882"/>
<move id="9" angle="-131.98882"/>
<move id="10" angle="123.69923"/>
<move id="11" angle="123.69923"/>
<move id="12" angle="-8.521371"/>
<move id="13" angle="-8.521371"/>
<move id="14" angle="3.173905"/>
<move id="15" angle="3.173905"/>
<move id="16" angle="-19.583878"/>
<move id="17" angle="-19.583878"/>
<move id="18" angle="-13.1733055"/>
<move id="19" angle="-13.1733055"/>
<move id="20" angle="-17.131882"/>
<move id="21" angle="-17.131882"/>
</slot>
<slot delta="0.06">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-19.195496"/>
<move id="3" angle="-19.195496"/>
<move id="4" angle="42.399166"/>
<move id="5" angle="42.399166"/>
<move id="6" angle="4.278442"/>
<move id="7" angle="4.278442"/>
<move id="8" angle="-93.31827"/>
<move id="9" angle="-93.31827"/>
<move id="10" angle="2.5259771"/>
<move id="11" angle="2.5259771"/>
<move id="12" angle="-33.76639"/>
<move id="13" angle="-33.76639"/>
<move id="14" angle="23.958351"/>
<move id="15" angle="23.958351"/>
<move id="16" angle="-7.903243"/>
<move id="17" angle="-7.903243"/>
<move id="18" angle="-18.376505"/>
<move id="19" angle="-18.376505"/>
<move id="20" angle="-20.77221"/>
<move id="21" angle="-20.77221"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-87.61395"/>
<move id="3" angle="-87.61395"/>
<move id="4" angle="-3.732545"/>
<move id="5" angle="-3.732545"/>
<move id="6" angle="-3.1527936"/>
<move id="7" angle="-3.1527936"/>
<move id="8" angle="-97.287674"/>
<move id="9" angle="-97.287674"/>
<move id="10" angle="-46.131386"/>
<move id="11" angle="-46.131386"/>
<move id="12" angle="-25.069555"/>
<move id="13" angle="-25.069555"/>
<move id="14" angle="-7.6755543"/>
<move id="15" angle="-7.6755543"/>
<move id="16" angle="-9.066617"/>
<move id="17" angle="-9.066617"/>
<move id="18" angle="9.437028"/>
<move id="19" angle="9.437028"/>
<move id="20" angle="0.19799754"/>
<move id="21" angle="0.19799754"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-4.7361717"/>
<move id="3" angle="-4.7361717"/>
<move id="4" angle="4.523938"/>
<move id="5" angle="4.523938"/>
<move id="6" angle="1.5581197"/>
<move id="7" angle="1.5581197"/>
<move id="8" angle="0.91717005"/>
<move id="9" angle="0.91717005"/>
<move id="10" angle="-0.73723245"/>
<move id="11" angle="-0.73723245"/>
<move id="12" angle="7.8827386"/>
<move id="13" angle="7.8827386"/>
<move id="14" angle="42.908276"/>
<move id="15" angle="42.908276"/>
<move id="16" angle="6.8883567"/>
<move id="17" angle="6.8883567"/>
<move id="18" angle="4.924833"/>
<move id="19" angle="4.924833"/>
<move id="20" angle="-0.006686151"/>
<move id="21" angle="-0.006686151"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-62.821877"/>
<move id="3" angle="-62.821877"/>
<move id="4" angle="1.0078714"/>
<move id="5" angle="1.0078714"/>
<move id="6" angle="1.994555"/>
<move id="7" angle="1.994555"/>
<move id="8" angle="-124.40493"/>
<move id="9" angle="-124.40493"/>
<move id="10" angle="46.153664"/>
<move id="11" angle="46.153664"/>
<move id="12" angle="30.022522"/>
<move id="13" angle="30.022522"/>
<move id="14" angle="3.9094872"/>
<move id="15" angle="3.9094872"/>
<move id="16" angle="12.2626095"/>
<move id="17" angle="12.2626095"/>
<move id="18" angle="12.774431"/>
<move id="19" angle="12.774431"/>
<move id="20" angle="10.435047"/>
<move id="21" angle="10.435047"/>
</slot>
<slot delta="0.06">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-55.784325"/>
<move id="3" angle="-55.784325"/>
<move id="4" angle="47.548805"/>
<move id="5" angle="47.548805"/>
<move id="6" angle="101.01688"/>
<move id="7" angle="101.01688"/>
<move id="8" angle="-122.21617"/>
<move id="9" angle="-122.21617"/>
<move id="10" angle="46.225307"/>
<move id="11" angle="46.225307"/>
<move id="12" angle="-56.009483"/>
<move id="13" angle="-56.009483"/>
<move id="14" angle="16.3225"/>
<move id="15" angle="16.3225"/>
<move id="16" angle="4.7965903"/>
<move id="17" angle="4.7965903"/>
<move id="18" angle="-25.914183"/>
<move id="19" angle="-25.914183"/>
<move id="20" angle="2.689302"/>
<move id="21" angle="2.689302"/>
</slot>
<slot delta="0.14">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-58.72785"/>
<move id="3" angle="-58.72785"/>
<move id="4" angle="43.85045"/>
<move id="5" angle="43.85045"/>
<move id="6" angle="108.34738"/>
<move id="7" angle="108.34738"/>
<move id="8" angle="-131.66202"/>
<move id="9" angle="-131.66202"/>
<move id="10" angle="44.46511"/>
<move id="11" angle="44.46511"/>
<move id="12" angle="-53.461926"/>
<move id="13" angle="-53.461926"/>
<move id="14" angle="-17.58822"/>
<move id="15" angle="-17.58822"/>
<move id="16" angle="-16.696602"/>
<move id="17" angle="-16.696602"/>
<move id="18" angle="0.24892278"/>
<move id="19" angle="0.24892278"/>
<move id="20" angle="14.703634"/>
<move id="21" angle="14.703634"/>
</slot>
<slot delta="0.42">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-1.136161"/>
<move id="3" angle="-1.136161"/>
<move id="4" angle="28.925074"/>
<move id="5" angle="28.925074"/>
<move id="6" angle="35.894753"/>
<move id="7" angle="35.894753"/>
<move id="8" angle="-94.40036"/>
<move id="9" angle="-94.40036"/>
<move id="10" angle="57.54773"/>
<move id="11" angle="57.54773"/>
<move id="12" angle="-4.952044"/>
<move id="13" angle="-4.952044"/>
<move id="14" angle="-123.39889"/>
<move id="15" angle="-123.39889"/>
<move id="16" angle="18.326159"/>
<move id="17" angle="18.326159"/>
<move id="18" angle="65.677376"/>
<move id="19" angle="65.677376"/>
<move id="20" angle="-21.766216"/>
<move id="21" angle="-21.766216"/>
</slot>
</behavior>

21
behaviors/slot/r2/Kick_Motion.xml
Unescape Escape View File

@ -0,0 +1,21 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Kick motion with right leg" auto_head="1">
<slot delta="0.22"> <!-- Lean -->
<move id="5" angle="-10"/> <!-- Left leg roll -->
<move id="6" angle="40"/> <!-- Left leg pitch -->
<move id="7" angle="65"/> <!-- Right leg pitch -->
<move id="8" angle="-60"/> <!-- Left knee -->
<move id="9" angle="-115"/> <!-- Right knee -->
<move id="10" angle="60"/> <!-- Left foot pitch -->
<move id="11" angle="25"/> <!-- Right foot pitch -->
</slot>
<slot delta="0.12"> <!-- Kick -->
<move id="3" angle="-45"/> <!-- Right leg yaw/pitch -->
<move id="6" angle="-25"/> <!-- Left leg pitch -->
<move id="7" angle="80"/> <!-- Right leg pitch -->
<move id="8" angle="0"/> <!-- Left knee -->
<move id="9" angle="0"/> <!-- Right knee -->
<move id="10" angle="30"/> <!-- Left foot pitch -->
</slot>
</behavior>

148
behaviors/slot/r3/Get_Up_Back.xml
Unescape Escape View File

@ -0,0 +1,148 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Get up from robot's back starting from Zero pose" auto_head="0">
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-38.572456"/>
<move id="3" angle="-38.572456"/>
<move id="4" angle="10.999639"/>
<move id="5" angle="10.999639"/>
<move id="6" angle="77.6285"/>
<move id="7" angle="77.6285"/>
<move id="8" angle="-15.158332"/>
<move id="9" angle="-15.158332"/>
<move id="10" angle="-35.486084"/>
<move id="11" angle="-35.486084"/>
<move id="12" angle="2.6835575"/>
<move id="13" angle="2.6835575"/>
<move id="14" angle="73.84404"/>
<move id="15" angle="73.84404"/>
<move id="16" angle="-54.934566"/>
<move id="17" angle="-54.934566"/>
<move id="18" angle="-113.41597"/>
<move id="19" angle="-113.41597"/>
<move id="20" angle="78.12353"/>
<move id="21" angle="78.12353"/>
</slot>
<slot delta="0.24">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-65.98044"/>
<move id="3" angle="-65.98044"/>
<move id="4" angle="-50.182472"/>
<move id="5" angle="-50.182472"/>
<move id="6" angle="30.041952"/>
<move id="7" angle="30.041952"/>
<move id="8" angle="34.80442"/>
<move id="9" angle="34.80442"/>
<move id="10" angle="49.432697"/>
<move id="11" angle="49.432697"/>
<move id="12" angle="-77.979546"/>
<move id="13" angle="-77.979546"/>
<move id="14" angle="-0.92560226"/>
<move id="15" angle="-0.92560226"/>
<move id="16" angle="52.747627"/>
<move id="17" angle="52.747627"/>
<move id="18" angle="127.680466"/>
<move id="19" angle="127.680466"/>
<move id="20" angle="57.550724"/>
<move id="21" angle="57.550724"/>
</slot>
<slot delta="0.32">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-62.44547"/>
<move id="3" angle="-62.44547"/>
<move id="4" angle="9.1681"/>
<move id="5" angle="9.1681"/>
<move id="6" angle="134.78099"/>
<move id="7" angle="134.78099"/>
<move id="8" angle="-27.68505"/>
<move id="9" angle="-27.68505"/>
<move id="10" angle="-115.14692"/>
<move id="11" angle="-115.14692"/>
<move id="12" angle="-44.91786"/>
<move id="13" angle="-44.91786"/>
<move id="14" angle="56.92144"/>
<move id="15" angle="56.92144"/>
<move id="16" angle="-52.54726"/>
<move id="17" angle="-52.54726"/>
<move id="18" angle="-54.15026"/>
<move id="19" angle="-54.15026"/>
<move id="20" angle="-9.677674"/>
<move id="21" angle="-9.677674"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-58.012352"/>
<move id="3" angle="-58.012352"/>
<move id="4" angle="43.231464"/>
<move id="5" angle="43.231464"/>
<move id="6" angle="-44.049873"/>
<move id="7" angle="-44.049873"/>
<move id="8" angle="-63.7723"/>
<move id="9" angle="-63.7723"/>
<move id="10" angle="-56.622414"/>
<move id="11" angle="-56.622414"/>
<move id="12" angle="-28.760904"/>
<move id="13" angle="-28.760904"/>
<move id="14" angle="-14.939745"/>
<move id="15" angle="-14.939745"/>
<move id="16" angle="23.633577"/>
<move id="17" angle="23.633577"/>
<move id="18" angle="-85.10401"/>
<move id="19" angle="-85.10401"/>
<move id="20" angle="17.66445"/>
<move id="21" angle="17.66445"/>
</slot>
<slot delta="0.06">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-24.942818"/>
<move id="3" angle="-24.942818"/>
<move id="4" angle="78.45739"/>
<move id="5" angle="78.45739"/>
<move id="6" angle="106.30913"/>
<move id="7" angle="106.30913"/>
<move id="8" angle="-152.12027"/>
<move id="9" angle="-152.12027"/>
<move id="10" angle="42.071213"/>
<move id="11" angle="42.071213"/>
<move id="12" angle="-65.846405"/>
<move id="13" angle="-65.846405"/>
<move id="14" angle="-57.11586"/>
<move id="15" angle="-57.11586"/>
<move id="16" angle="-81.987076"/>
<move id="17" angle="-81.987076"/>
<move id="18" angle="11.453229"/>
<move id="19" angle="11.453229"/>
<move id="20" angle="-39.00894"/>
<move id="21" angle="-39.00894"/>
</slot>
<slot delta="0.18">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-0.6814623"/>
<move id="3" angle="-0.6814623"/>
<move id="4" angle="35.28643"/>
<move id="5" angle="35.28643"/>
<move id="6" angle="75.47946"/>
<move id="7" angle="75.47946"/>
<move id="8" angle="-132.41306"/>
<move id="9" angle="-132.41306"/>
<move id="10" angle="46.746563"/>
<move id="11" angle="46.746563"/>
<move id="12" angle="-8.374908"/>
<move id="13" angle="-8.374908"/>
<move id="14" angle="-20.005192"/>
<move id="15" angle="-20.005192"/>
<move id="16" angle="9.418695"/>
<move id="17" angle="9.418695"/>
<move id="18" angle="186.1502"/>
<move id="19" angle="186.1502"/>
<move id="20" angle="133.16562"/>
<move id="21" angle="133.16562"/>
</slot>
</behavior>

220
behaviors/slot/r3/Get_Up_Front.xml
Unescape Escape View File

@ -0,0 +1,220 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Get up from robot's front starting from Zero pose" auto_head="0">
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-12.640889"/>
<move id="3" angle="-12.640889"/>
<move id="4" angle="4.646634"/>
<move id="5" angle="4.646634"/>
<move id="6" angle="17.342707"/>
<move id="7" angle="17.342707"/>
<move id="8" angle="3.5387506"/>
<move id="9" angle="3.5387506"/>
<move id="10" angle="3.3997679"/>
<move id="11" angle="3.3997679"/>
<move id="12" angle="0.6578274"/>
<move id="13" angle="0.6578274"/>
<move id="14" angle="-8.817135"/>
<move id="15" angle="-8.817135"/>
<move id="16" angle="61.96675"/>
<move id="17" angle="61.96675"/>
<move id="18" angle="1.5812168"/>
<move id="19" angle="1.5812168"/>
<move id="20" angle="-8.415477"/>
<move id="21" angle="-8.415477"/>
</slot>
<slot delta="0.18">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="5.4146767"/>
<move id="3" angle="5.4146767"/>
<move id="4" angle="-11.824036"/>
<move id="5" angle="-11.824036"/>
<move id="6" angle="8.015917"/>
<move id="7" angle="8.015917"/>
<move id="8" angle="-1.4786508"/>
<move id="9" angle="-1.4786508"/>
<move id="10" angle="6.2315826"/>
<move id="11" angle="6.2315826"/>
<move id="12" angle="11.371917"/>
<move id="13" angle="11.371917"/>
<move id="14" angle="6.024707"/>
<move id="15" angle="6.024707"/>
<move id="16" angle="61.55641"/>
<move id="17" angle="61.55641"/>
<move id="18" angle="-6.719823"/>
<move id="19" angle="-6.719823"/>
<move id="20" angle="-19.866516"/>
<move id="21" angle="-19.866516"/>
</slot>
<slot delta="0.06">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-2.2857966"/>
<move id="3" angle="-2.2857966"/>
<move id="4" angle="-5.441041"/>
<move id="5" angle="-5.441041"/>
<move id="6" angle="270.0556"/>
<move id="7" angle="270.0556"/>
<move id="8" angle="-127.3845"/>
<move id="9" angle="-127.3845"/>
<move id="10" angle="123.612"/>
<move id="11" angle="123.612"/>
<move id="12" angle="-6.389326"/>
<move id="13" angle="-6.389326"/>
<move id="14" angle="-2.5006394"/>
<move id="15" angle="-2.5006394"/>
<move id="16" angle="-8.59928"/>
<move id="17" angle="-8.59928"/>
<move id="18" angle="-10.413264"/>
<move id="19" angle="-10.413264"/>
<move id="20" angle="-10.369881"/>
<move id="21" angle="-10.369881"/>
</slot>
<slot delta="0.06">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-15.020902"/>
<move id="3" angle="-15.020902"/>
<move id="4" angle="39.801743"/>
<move id="5" angle="39.801743"/>
<move id="6" angle="7.683368"/>
<move id="7" angle="7.683368"/>
<move id="8" angle="-96.00759"/>
<move id="9" angle="-96.00759"/>
<move id="10" angle="2.5151913"/>
<move id="11" angle="2.5151913"/>
<move id="12" angle="-44.600082"/>
<move id="13" angle="-44.600082"/>
<move id="14" angle="28.740438"/>
<move id="15" angle="28.740438"/>
<move id="16" angle="-7.8307633"/>
<move id="17" angle="-7.8307633"/>
<move id="18" angle="-25.14883"/>
<move id="19" angle="-25.14883"/>
<move id="20" angle="-21.899431"/>
<move id="21" angle="-21.899431"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-91.19676"/>
<move id="3" angle="-91.19676"/>
<move id="4" angle="-1.6109102"/>
<move id="5" angle="-1.6109102"/>
<move id="6" angle="-3.0909305"/>
<move id="7" angle="-3.0909305"/>
<move id="8" angle="-97.02908"/>
<move id="9" angle="-97.02908"/>
<move id="10" angle="-44.9949"/>
<move id="11" angle="-44.9949"/>
<move id="12" angle="-24.864588"/>
<move id="13" angle="-24.864588"/>
<move id="14" angle="-10.729849"/>
<move id="15" angle="-10.729849"/>
<move id="16" angle="-12.871218"/>
<move id="17" angle="-12.871218"/>
<move id="18" angle="10.0304985"/>
<move id="19" angle="10.0304985"/>
<move id="20" angle="10.550584"/>
<move id="21" angle="10.550584"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-3.6664605"/>
<move id="3" angle="-3.6664605"/>
<move id="4" angle="2.7512116"/>
<move id="5" angle="2.7512116"/>
<move id="6" angle="4.2976775"/>
<move id="7" angle="4.2976775"/>
<move id="8" angle="-1.8928692"/>
<move id="9" angle="-1.8928692"/>
<move id="10" angle="-1.3523235"/>
<move id="11" angle="-1.3523235"/>
<move id="12" angle="-4.7275023"/>
<move id="13" angle="-4.7275023"/>
<move id="14" angle="44.758335"/>
<move id="15" angle="44.758335"/>
<move id="16" angle="17.067863"/>
<move id="17" angle="17.067863"/>
<move id="18" angle="-7.7519336"/>
<move id="19" angle="-7.7519336"/>
<move id="20" angle="-4.652607"/>
<move id="21" angle="-4.652607"/>
</slot>
<slot delta="0.04">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-59.41754"/>
<move id="3" angle="-59.41754"/>
<move id="4" angle="49.316"/>
<move id="5" angle="49.316"/>
<move id="6" angle="100.66388"/>
<move id="7" angle="100.66388"/>
<move id="8" angle="-121.67775"/>
<move id="9" angle="-121.67775"/>
<move id="10" angle="46.480865"/>
<move id="11" angle="46.480865"/>
<move id="12" angle="-57.7324"/>
<move id="13" angle="-57.7324"/>
<move id="14" angle="5.5761175"/>
<move id="15" angle="5.5761175"/>
<move id="16" angle="18.378542"/>
<move id="17" angle="18.378542"/>
<move id="18" angle="-27.80392"/>
<move id="19" angle="-27.80392"/>
<move id="20" angle="10.984678"/>
<move id="21" angle="10.984678"/>
</slot>
<slot delta="0.12">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-47.20175"/>
<move id="3" angle="-47.20175"/>
<move id="4" angle="47.4648"/>
<move id="5" angle="47.4648"/>
<move id="6" angle="115.67783"/>
<move id="7" angle="115.67783"/>
<move id="8" angle="-135.32626"/>
<move id="9" angle="-135.32626"/>
<move id="10" angle="42.217873"/>
<move id="11" angle="42.217873"/>
<move id="12" angle="-65.463715"/>
<move id="13" angle="-65.463715"/>
<move id="14" angle="-19.853582"/>
<move id="15" angle="-19.853582"/>
<move id="16" angle="-26.5262"/>
<move id="17" angle="-26.5262"/>
<move id="18" angle="17.335098"/>
<move id="19" angle="17.335098"/>
<move id="20" angle="7.6011276"/>
<move id="21" angle="7.6011276"/>
</slot>
<slot delta="0.4">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-1.6223584"/>
<move id="3" angle="-1.6223584"/>
<move id="4" angle="24.222506"/>
<move id="5" angle="24.222506"/>
<move id="6" angle="38.50319"/>
<move id="7" angle="38.50319"/>
<move id="8" angle="-93.59837"/>
<move id="9" angle="-93.59837"/>
<move id="10" angle="53.80671"/>
<move id="11" angle="53.80671"/>
<move id="12" angle="-2.3789244"/>
<move id="13" angle="-2.3789244"/>
<move id="14" angle="-119.351814"/>
<move id="15" angle="-119.351814"/>
<move id="16" angle="0.0"/>
<move id="17" angle="0.0"/>
<move id="18" angle="67.36445"/>
<move id="19" angle="67.36445"/>
<move id="20" angle="90"/>
<move id="21" angle="90"/>
</slot>
</behavior>

22
behaviors/slot/r3/Kick_Motion.xml
Unescape Escape View File

@ -0,0 +1,22 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Kick motion with right leg" auto_head="1">
<slot delta="0.22"> <!-- Lean -->
<move id="5" angle="-10"/> <!-- Left leg roll -->
<move id="6" angle="40"/> <!-- Left leg pitch -->
<move id="7" angle="65"/> <!-- Right leg pitch -->
<move id="8" angle="-60"/> <!-- Left knee -->
<move id="9" angle="-115"/> <!-- Right knee -->
<move id="10" angle="60"/> <!-- Left foot pitch -->
<move id="11" angle="-5"/> <!-- Right foot pitch -->
</slot>
<slot delta="0.12"> <!-- Kick -->
<move id="3" angle="-45"/> <!-- Right leg yaw/pitch -->
<move id="6" angle="-25"/> <!-- Left leg pitch -->
<move id="7" angle="80"/> <!-- Right leg pitch -->
<move id="8" angle="0"/> <!-- Left knee -->
<move id="9" angle="0"/> <!-- Right knee -->
<move id="10" angle="30"/> <!-- Left foot pitch -->
<move id="17" angle="40"/> <!-- Right arm roll -->
</slot>
</behavior>

148
behaviors/slot/r4/Get_Up_Back.xml
Unescape Escape View File

@ -0,0 +1,148 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Get up from robot's back starting from Zero pose" auto_head="0">
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-48.405083"/>
<move id="3" angle="-48.405083"/>
<move id="4" angle="33.774643"/>
<move id="5" angle="33.774643"/>
<move id="6" angle="100.784706"/>
<move id="7" angle="100.784706"/>
<move id="8" angle="7.756978"/>
<move id="9" angle="7.756978"/>
<move id="10" angle="-23.10689"/>
<move id="11" angle="-23.10689"/>
<move id="12" angle="85.49136"/>
<move id="13" angle="85.49136"/>
<move id="14" angle="38.31756"/>
<move id="15" angle="38.31756"/>
<move id="16" angle="-67.331825"/>
<move id="17" angle="-67.331825"/>
<move id="18" angle="22.26954"/>
<move id="19" angle="22.26954"/>
<move id="20" angle="74.803024"/>
<move id="21" angle="74.803024"/>
</slot>
<slot delta="0.23336456513404846">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-70.5122"/>
<move id="3" angle="-70.5122"/>
<move id="4" angle="-33.731937"/>
<move id="5" angle="-33.731937"/>
<move id="6" angle="24.08416"/>
<move id="7" angle="24.08416"/>
<move id="8" angle="43.438618"/>
<move id="9" angle="43.438618"/>
<move id="10" angle="58.60381"/>
<move id="11" angle="58.60381"/>
<move id="12" angle="-85.92165"/>
<move id="13" angle="-85.92165"/>
<move id="14" angle="5.59869"/>
<move id="15" angle="5.59869"/>
<move id="16" angle="122.71095"/>
<move id="17" angle="122.71095"/>
<move id="18" angle="55.634182"/>
<move id="19" angle="55.634182"/>
<move id="20" angle="85.27185"/>
<move id="21" angle="85.27185"/>
</slot>
<slot delta="0.33675475766658775">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-38.059784"/>
<move id="3" angle="-38.059784"/>
<move id="4" angle="46.120163"/>
<move id="5" angle="46.120163"/>
<move id="6" angle="120.91053"/>
<move id="7" angle="120.91053"/>
<move id="8" angle="-37.19999"/>
<move id="9" angle="-37.19999"/>
<move id="10" angle="-60.143646"/>
<move id="11" angle="-60.143646"/>
<move id="12" angle="-81.65832"/>
<move id="13" angle="-81.65832"/>
<move id="14" angle="35.256054"/>
<move id="15" angle="35.256054"/>
<move id="16" angle="-17.517155"/>
<move id="17" angle="-17.517155"/>
<move id="18" angle="18.431877"/>
<move id="19" angle="18.431877"/>
<move id="20" angle="-25.010399"/>
<move id="21" angle="-25.010399"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-69.23109"/>
<move id="3" angle="-69.23109"/>
<move id="4" angle="15.320035"/>
<move id="5" angle="15.320035"/>
<move id="6" angle="30.714127"/>
<move id="7" angle="30.714127"/>
<move id="8" angle="-47.507515"/>
<move id="9" angle="-47.507515"/>
<move id="10" angle="-61.518223"/>
<move id="11" angle="-61.518223"/>
<move id="12" angle="-27.325377"/>
<move id="13" angle="-27.325377"/>
<move id="14" angle="45.20161"/>
<move id="15" angle="45.20161"/>
<move id="16" angle="71.49224"/>
<move id="17" angle="71.49224"/>
<move id="18" angle="37.682297"/>
<move id="19" angle="37.682297"/>
<move id="20" angle="10.35512"/>
<move id="21" angle="10.35512"/>
</slot>
<slot delta="0.05201605577468872">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-28.16217"/>
<move id="3" angle="-28.16217"/>
<move id="4" angle="52.758217"/>
<move id="5" angle="52.758217"/>
<move id="6" angle="111.2359"/>
<move id="7" angle="111.2359"/>
<move id="8" angle="-118.02957"/>
<move id="9" angle="-118.02957"/>
<move id="10" angle="54.038685"/>
<move id="11" angle="54.038685"/>
<move id="12" angle="-123.210915"/>
<move id="13" angle="-123.210915"/>
<move id="14" angle="-26.220789"/>
<move id="15" angle="-26.220789"/>
<move id="16" angle="28.817947"/>
<move id="17" angle="28.817947"/>
<move id="18" angle="-14.253399"/>
<move id="19" angle="-14.253399"/>
<move id="20" angle="17.878008"/>
<move id="21" angle="17.878008"/>
</slot>
<slot delta="0.15276842055320738">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-5.5297728"/>
<move id="3" angle="-5.5297728"/>
<move id="4" angle="90.15973"/>
<move id="5" angle="90.15973"/>
<move id="6" angle="50.418648"/>
<move id="7" angle="50.418648"/>
<move id="8" angle="-90.911835"/>
<move id="9" angle="-90.911835"/>
<move id="10" angle="50.15513"/>
<move id="11" angle="50.15513"/>
<move id="12" angle="-45.83729"/>
<move id="13" angle="-45.83729"/>
<move id="14" angle="-39.350525"/>
<move id="15" angle="-39.350525"/>
<move id="16" angle="-23.518953"/>
<move id="17" angle="-23.518953"/>
<move id="18" angle="72.38919"/>
<move id="19" angle="72.38919"/>
<move id="20" angle="-77.339966"/>
<move id="21" angle="-77.339966"/>
</slot>
</behavior>

220
behaviors/slot/r4/Get_Up_Front.xml
Unescape Escape View File

@ -0,0 +1,220 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Get up from robot's front starting from Zero pose" auto_head="0">
<slot delta="0.16">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="4.6313915"/>
<move id="3" angle="4.6313915"/>
<move id="4" angle="-11.846246"/>
<move id="5" angle="-11.846246"/>
<move id="6" angle="8.745557"/>
<move id="7" angle="8.745557"/>
<move id="8" angle="-1.8197118"/>
<move id="9" angle="-1.8197118"/>
<move id="10" angle="1.359597"/>
<move id="11" angle="1.359597"/>
<move id="12" angle="16.82137"/>
<move id="13" angle="16.82137"/>
<move id="14" angle="21.83775"/>
<move id="15" angle="21.83775"/>
<move id="16" angle="-27.174564"/>
<move id="17" angle="-27.174564"/>
<move id="18" angle="-12.889872"/>
<move id="19" angle="-12.889872"/>
<move id="20" angle="-14.989797"/>
<move id="21" angle="-14.989797"/>
</slot>
<slot delta="0.08">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="1.0406766"/>
<move id="3" angle="1.0406766"/>
<move id="4" angle="-7.939904"/>
<move id="5" angle="-7.939904"/>
<move id="6" angle="260.59564"/>
<move id="7" angle="260.59564"/>
<move id="8" angle="-131.98882"/>
<move id="9" angle="-131.98882"/>
<move id="10" angle="123.69923"/>
<move id="11" angle="123.69923"/>
<move id="12" angle="-8.521371"/>
<move id="13" angle="-8.521371"/>
<move id="14" angle="3.173905"/>
<move id="15" angle="3.173905"/>
<move id="16" angle="-19.583878"/>
<move id="17" angle="-19.583878"/>
<move id="18" angle="-13.1733055"/>
<move id="19" angle="-13.1733055"/>
<move id="20" angle="-17.131882"/>
<move id="21" angle="-17.131882"/>
</slot>
<slot delta="0.06">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-19.195496"/>
<move id="3" angle="-19.195496"/>
<move id="4" angle="42.399166"/>
<move id="5" angle="42.399166"/>
<move id="6" angle="4.278442"/>
<move id="7" angle="4.278442"/>
<move id="8" angle="-93.31827"/>
<move id="9" angle="-93.31827"/>
<move id="10" angle="2.5259771"/>
<move id="11" angle="2.5259771"/>
<move id="12" angle="-33.76639"/>
<move id="13" angle="-33.76639"/>
<move id="14" angle="23.958351"/>
<move id="15" angle="23.958351"/>
<move id="16" angle="-7.903243"/>
<move id="17" angle="-7.903243"/>
<move id="18" angle="-18.376505"/>
<move id="19" angle="-18.376505"/>
<move id="20" angle="-20.77221"/>
<move id="21" angle="-20.77221"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-87.61395"/>
<move id="3" angle="-87.61395"/>
<move id="4" angle="-3.732545"/>
<move id="5" angle="-3.732545"/>
<move id="6" angle="-3.1527936"/>
<move id="7" angle="-3.1527936"/>
<move id="8" angle="-97.287674"/>
<move id="9" angle="-97.287674"/>
<move id="10" angle="-46.131386"/>
<move id="11" angle="-46.131386"/>
<move id="12" angle="-25.069555"/>
<move id="13" angle="-25.069555"/>
<move id="14" angle="-7.6755543"/>
<move id="15" angle="-7.6755543"/>
<move id="16" angle="-9.066617"/>
<move id="17" angle="-9.066617"/>
<move id="18" angle="9.437028"/>
<move id="19" angle="9.437028"/>
<move id="20" angle="0.19799754"/>
<move id="21" angle="0.19799754"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-4.7361717"/>
<move id="3" angle="-4.7361717"/>
<move id="4" angle="4.523938"/>
<move id="5" angle="4.523938"/>
<move id="6" angle="1.5581197"/>
<move id="7" angle="1.5581197"/>
<move id="8" angle="0.91717005"/>
<move id="9" angle="0.91717005"/>
<move id="10" angle="-0.73723245"/>
<move id="11" angle="-0.73723245"/>
<move id="12" angle="7.8827386"/>
<move id="13" angle="7.8827386"/>
<move id="14" angle="42.908276"/>
<move id="15" angle="42.908276"/>
<move id="16" angle="6.8883567"/>
<move id="17" angle="6.8883567"/>
<move id="18" angle="4.924833"/>
<move id="19" angle="4.924833"/>
<move id="20" angle="-0.006686151"/>
<move id="21" angle="-0.006686151"/>
</slot>
<slot delta="0.02">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-62.821877"/>
<move id="3" angle="-62.821877"/>
<move id="4" angle="1.0078714"/>
<move id="5" angle="1.0078714"/>
<move id="6" angle="1.994555"/>
<move id="7" angle="1.994555"/>
<move id="8" angle="-124.40493"/>
<move id="9" angle="-124.40493"/>
<move id="10" angle="46.153664"/>
<move id="11" angle="46.153664"/>
<move id="12" angle="30.022522"/>
<move id="13" angle="30.022522"/>
<move id="14" angle="3.9094872"/>
<move id="15" angle="3.9094872"/>
<move id="16" angle="12.2626095"/>
<move id="17" angle="12.2626095"/>
<move id="18" angle="12.774431"/>
<move id="19" angle="12.774431"/>
<move id="20" angle="10.435047"/>
<move id="21" angle="10.435047"/>
</slot>
<slot delta="0.06">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-55.784325"/>
<move id="3" angle="-55.784325"/>
<move id="4" angle="47.548805"/>
<move id="5" angle="47.548805"/>
<move id="6" angle="101.01688"/>
<move id="7" angle="101.01688"/>
<move id="8" angle="-122.21617"/>
<move id="9" angle="-122.21617"/>
<move id="10" angle="46.225307"/>
<move id="11" angle="46.225307"/>
<move id="12" angle="-56.009483"/>
<move id="13" angle="-56.009483"/>
<move id="14" angle="16.3225"/>
<move id="15" angle="16.3225"/>
<move id="16" angle="4.7965903"/>
<move id="17" angle="4.7965903"/>
<move id="18" angle="-25.914183"/>
<move id="19" angle="-25.914183"/>
<move id="20" angle="2.689302"/>
<move id="21" angle="2.689302"/>
</slot>
<slot delta="0.14">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-58.72785"/>
<move id="3" angle="-58.72785"/>
<move id="4" angle="43.85045"/>
<move id="5" angle="43.85045"/>
<move id="6" angle="108.34738"/>
<move id="7" angle="108.34738"/>
<move id="8" angle="-131.66202"/>
<move id="9" angle="-131.66202"/>
<move id="10" angle="44.46511"/>
<move id="11" angle="44.46511"/>
<move id="12" angle="-53.461926"/>
<move id="13" angle="-53.461926"/>
<move id="14" angle="-17.58822"/>
<move id="15" angle="-17.58822"/>
<move id="16" angle="-16.696602"/>
<move id="17" angle="-16.696602"/>
<move id="18" angle="0.24892278"/>
<move id="19" angle="0.24892278"/>
<move id="20" angle="14.703634"/>
<move id="21" angle="14.703634"/>
</slot>
<slot delta="0.42">
<move id="0" angle="0.0"/>
<move id="1" angle="0.0"/>
<move id="2" angle="-1.136161"/>
<move id="3" angle="-1.136161"/>
<move id="4" angle="28.925074"/>
<move id="5" angle="28.925074"/>
<move id="6" angle="35.894753"/>
<move id="7" angle="35.894753"/>
<move id="8" angle="-94.40036"/>
<move id="9" angle="-94.40036"/>
<move id="10" angle="57.54773"/>
<move id="11" angle="57.54773"/>
<move id="12" angle="-4.952044"/>
<move id="13" angle="-4.952044"/>
<move id="14" angle="-123.39889"/>
<move id="15" angle="-123.39889"/>
<move id="16" angle="18.326159"/>
<move id="17" angle="18.326159"/>
<move id="18" angle="65.677376"/>
<move id="19" angle="65.677376"/>
<move id="20" angle="-21.766216"/>
<move id="21" angle="-21.766216"/>
</slot>
</behavior>

21
behaviors/slot/r4/Kick_Motion.xml
Unescape Escape View File

@ -0,0 +1,21 @@
<?xml version="1.0" encoding="utf-8"?>
<behavior description="Kick motion with right leg" auto_head="1">
<slot delta="0.18"> <!-- Lean -->
<move id="5" angle="-5"/> <!-- Left leg roll -->
<move id="7" angle="65"/> <!-- Right leg pitch -->
<move id="8" angle="-70"/> <!-- Left knee -->
<move id="9" angle="-130"/> <!-- Right knee -->
<move id="10" angle="60"/> <!-- Left foot pitch -->
<move id="11" angle="30"/> <!-- Right foot pitch -->
</slot>
<slot delta="0.1"> <!-- Kick -->
<move id="3" angle="-45"/> <!-- Right leg yaw/pitch -->
<move id="6" angle="-25"/> <!-- Left leg pitch -->
<move id="7" angle="100"/> <!-- Right leg pitch -->
<move id="8" angle="0"/> <!-- Left knee -->
<move id="9" angle="0"/> <!-- Right knee -->
<move id="10" angle="30"/> <!-- Left foot pitch -->
<move id="11" angle="25"/> <!-- Right foot pitch -->
</slot>
</behavior>

83
bundle/bundle.sh
Unescape Escape View File

@ -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

306
communication/Radio.py
Unescape Escape View File

@ -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

285
communication/Server_Comm.py
Unescape Escape View File

@ -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

430
communication/World_Parser.py
Unescape Escape View File

@ -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)

14
cpp/a_star/Makefile
Unescape Escape View File

@ -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

918
cpp/a_star/a_star.cpp
Unescape Escape View File

@ -0,0 +1,918 @@
#include "a_star.h"
#include "expansion_groups.h"
#include <cmath>
#include <algorithm>
#include <chrono>
#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<CUSHION_WIDTH; i++){
int ii = (i+12)*COLS;
for(int j=12+i; j<209-i; j++){
board_cost[ii+j] = CUSHION_WIDTH-i;
}
}
// our goal line
for(int i=0; i<CUSHION_WIDTH; i++){
int ii = (308-i)*COLS;
for(int j=12+i; j<99; j++){
board_cost[ii+j] = CUSHION_WIDTH-i;
board_cost[ii+220-j] = CUSHION_WIDTH-i;
}
}
// sidelines
for(int i=0; i<CUSHION_WIDTH; i++){
for(int j=(13+i)*COLS; j<(308-i)*COLS; j+=COLS){
board_cost[j+i+12] = CUSHION_WIDTH-i;
board_cost[j-i+208] = CUSHION_WIDTH-i;
}
}
}
/**
* @brief This function checks if a straight path from start to end is obstructed by obstacles
* Returning 'false' means that we do not need A* to solve this problem
*
* Normal case (start != end):
* The path is obstructed if it intersects any hard or soft circumference
* Special case (start == end):
* The path is obstructed if start is inside any hard circumference
*/
bool is_path_obstructed(float start_x, float start_y, float end_x, float end_y, float given_obstacles[],
int given_obst_size, bool go_to_goal, int wall_index, float board_cost[]){
// Restrict start coordinates to map
start_x = max(-16.f, min(start_x, 16.f));
start_y = max(-11.f, min(start_y, 11.f));
int s_lin = x_to_line(start_x);
int e_lin = x_to_line(end_x);
int s_col = y_to_col(start_y);
int e_col = y_to_col(end_y);
float s_cost = board_cost[s_lin*COLS+s_col];
float e_cost = board_cost[e_lin*COLS+e_col];
// Path is obvious if start/end are is same or adjacent cells
bool is_near = abs(s_lin - e_lin) <= 1 and abs(s_col - e_col) <= 1;
// Let A* handle it if the start position is unreachable or (out of bounds is not allowed && is nearly out of bounds && not near end)
if(s_cost <= wall_index or (!is_near and s_cost > 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; i<given_obst_size; i+=5){
obst[obst_size++] = given_obstacles[i]; // x
obst[obst_size++] = given_obstacles[i+1]; // y
obst[obst_size++] = fmaxf( 0, fminf(given_obstacles[i+2], MAX_RADIUS) ); // hard radius
obst[obst_size++] = fmaxf( 0, fminf(max(given_obstacles[i+2], given_obstacles[i+3]), MAX_RADIUS) ); // largest radius
}
//------------------------ Special case (start ~= end): the path is obstructed if start or end are inside any hard circumference
if( is_near ){
for(int ob=0; ob<obst_size; ob+=4){
float c_x = obst[ob]; // obstacle center
float c_y = obst[ob+1]; // obstacle center
float hard_radius = obst[ob+2]; // hard radius
float r_sq = hard_radius * hard_radius; // squared radius
float sc_x = c_x - start_x;
float sc_y = c_y - start_y;
float ec_x = c_x - end_x;
float ec_y = c_y - end_y;
if(sc_x*sc_x + sc_y*sc_y <= r_sq or ec_x*ec_x + ec_y*ec_y <= r_sq){ // check distance: center<->start 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; ob<obst_size; ob+=4){
float c_x = obst[ob]; // obstacle center
float c_y = obst[ob+1]; // obstacle center
float largest_radius = obst[ob+3]; // largest radius
float r_sq = largest_radius * largest_radius; // squared radius
float sc_x = c_x - start_x;
float sc_y = c_y - start_y;
float se_x = end_x - start_x;
float se_y = end_y - start_y;
float sc_proj_scale = (sc_x*se_x + sc_y*se_y) / (se_x*se_x + se_y*se_y); // scale = projection length / target vector length
float sc_proj_x = se_x * sc_proj_scale; // projection of start->center 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 = &params[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<obst_size; ob+=5){
int lin = x_to_line(obstacles[ob]);
int col = y_to_col(obstacles[ob+1]);
float hard_radius = fmaxf( 0, fminf(obstacles[ob+2], MAX_RADIUS) );
float soft_radius = fmaxf( 0, fminf(obstacles[ob+3], MAX_RADIUS) );
float force = obstacles[ob+4];
float f_per_m = force / soft_radius; // force per meter
int max_r = int( fmaxf(hard_radius, soft_radius)*10.f+1e-4 ); // add epsilon to avoid potential rounding error in expansion groups
l_min = min(l_min, lin - max_r - 1 );
l_max = max(l_max, lin + max_r + 1 );
c_min = min(c_min, col - max_r - 1 );
c_max = max(c_max, col + max_r + 1 );
//=============================================================== hard radius
int i=0;
for(; i<expansion_positions_no and expansion_pos_dist[i] <= hard_radius; i++){
int l = lin + expansion_pos_l[i];
int c = col + expansion_pos_c[i];
if(l>=0 and c>=0 and l<LINES and c<COLS){
board_cost[l*COLS+c] = -3;
}
}
//=============================================================== soft radius
for(; i<expansion_positions_no and expansion_pos_dist[i] <= soft_radius; i++){
int l = lin + expansion_pos_l[i];
int c = col + expansion_pos_c[i];
int p = l*COLS+c;
float cost = board_cost[p];
float fr = force-(f_per_m * expansion_pos_dist[i]);
if(l>=0 and c>=0 and l<LINES and c<COLS and cost > 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<float>::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<microseconds>(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;
}

26
cpp/a_star/a_star.h
Unescape Escape View File

@ -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;

37
cpp/a_star/debug_main.cc
Unescape Escape View File

@ -0,0 +1,37 @@
#include "a_star.h"
#include <chrono>
#include <iostream>
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<microseconds>(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<microseconds>(t2 - t1).count() << "us\n";
}

6
cpp/a_star/expansion_groups.h
View File

File diff suppressed because one or more lines are too long

60
cpp/a_star/expansion_groups.py
Unescape Escape View File

@ -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))

43
cpp/a_star/lib_main.cpp
Unescape Escape View File

@ -0,0 +1,43 @@
#include "a_star.h"
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
namespace py = pybind11;
using namespace std;
py::array_t<float> compute( py::array_t<float> 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<float> retval = py::array_t<float>(final_path_size); //allocate
py::buffer_info buff = retval.request();
float *ptr = (float *) buff.ptr;
for(int i=0; i<final_path_size; i++){
ptr[i] = final_path[i];
}
return retval;
}
using namespace pybind11::literals; // to add informative argument names as -> "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);
}

14
cpp/ball_predictor/Makefile
Unescape Escape View File

@ -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

104
cpp/ball_predictor/ball_predictor.cpp
Unescape Escape View File

@ -0,0 +1,104 @@
#include <cmath>
#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;
}

10
cpp/ball_predictor/ball_predictor.h
Unescape Escape View File

@ -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);

54
cpp/ball_predictor/debug_main.cc
Unescape Escape View File

@ -0,0 +1,54 @@
#include "ball_predictor.h"
#include <chrono>
#include <iostream>
#include <iomanip>
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<pos_pred_len; i+=2){
cout << i/2 << " pos:" << ball_pos_pred[i] << "," << ball_pos_pred[i+1] <<
" vel:" << ball_vel_pred[i] << "," << ball_vel_pred[i+1] <<
" spd:" << ball_spd_pred[i/2] << "\n";
}
cout << "\n\n" << duration_cast<microseconds>(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<microseconds>(t2 - t1).count() << "us for intersection\n\n";
cout << "Intersection: " << ret_x << "," << ret_y << " dist: " << ret_d << "\n\n";
}

100
cpp/ball_predictor/lib_main.cpp
Unescape Escape View File

@ -0,0 +1,100 @@
#include "ball_predictor.h"
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
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<float> predict_rolling_ball( py::array_t<float> 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<float> retval = py::array_t<float>(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<pos_pred_len; i++){
ptr[i] = ball_pos_pred[i];
}
ptr+=pos_pred_len;
for(int i=0; i<pos_pred_len; i++){
ptr[i] = ball_vel_pred[i];
}
ptr+=pos_pred_len;
for(int i=0; i<pos_pred_len/2; i++){
ptr[i] = ball_spd_pred[i];
}
return retval;
}
/**
* @brief Get point of intersection with moving ball
*
* @param parameters
* robot_x, robot_y, robot_max_speed_per_step
* @return intersection_x, intersection_y, intersection_distance
*/
py::array_t<float> get_intersection( py::array_t<float> 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<float> retval = py::array_t<float>(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);
}

532
cpp/localization/Field.cpp
Unescape Escape View File

@ -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<Line6f*> 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<x<0.5]=0.99996
* Noise applied vertically sigma=0.1480, Pr[-0.5<x<0.5]=0.99928
*
* Warning: the server limits the focal distance to 10cm (which is enforced in the x cartesian coordinate)
* current solution: lRelC[i].x > 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)
}

503
cpp/localization/Field.h
Unescape Escape View File

@ -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 <vector>
#include <array>
using namespace std;
class Field {
friend class Singleton<Field>;
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<sFieldPoint,28> 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<sFieldSegment,21> 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<sMarker> list_landmarks;
/**
* Visible corners
*/
vector<sMarker> list_landmarks_corners;
/**
* Visible goalposts
*/
vector<sMarker> 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<sSegment> list_known_segments;
/**
* Identified visible line segment endpoints + landmarks
* Each marker has a reference to the corresponding field point
*/
vector<sMarker> 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<sMarker> list_unknown_markers;
/**
* Visible line endpoints + foot contact points + corner flags (the absolute position is always (0,0,0))
*/
vector<sMarker> 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<sMarker> list_weighted_ground_markers;
/**
* Feet contact points
*/
vector<sMarker> list_feet_contact_points;
/**
* Visible line segments
*/
vector<Line6f> 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<sFixedMarker,8> 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<Field> SField;

104
cpp/localization/FieldNoise.cpp
Unescape Escape View File

@ -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;
}

91
cpp/localization/FieldNoise.h
Unescape Escape View File

@ -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);
};

344
cpp/localization/Geometry.cpp
Unescape Escape View File

@ -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;
}

71
cpp/localization/Geometry.h
Unescape Escape View File

@ -0,0 +1,71 @@
#ifndef GEOMETRY_H
#define GEOMETRY_H
#include <cmath>
#include <algorithm>
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

232
cpp/localization/Line6f.cpp
Unescape Escape View File

@ -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;
}

185
cpp/localization/Line6f.h
Unescape Escape View File

@ -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;
};

1249
cpp/localization/LocalizerV2.cpp
View File

File diff suppressed because it is too large Load Diff

379
cpp/localization/LocalizerV2.h
Unescape Escape View File

@ -0,0 +1,379 @@
/**
* FILENAME: LocalizerV2
* DESCRIPTION: main 6D localization algorithm
* AUTHOR: Miguel Abreu (m.abreu@fe.up.pt)
* DATE: 2021
*
* ===================================================================================
* WORKFLOW
* ===================================================================================
*
* References can be obtained from:
* - landmarks (which are identified by the server and can be corner flags or goal posts)
* - line segments (which are always on the ground) (hereinafter referred to as lines)
* - feet contact points (we are assuming the contact point is on the ground plane)
*
* WARNING:
* When partial information is available, it is used to change the head position (e.g. translation in x/y/z may be updated
* without visual information). However, the transformation matrix (including the translation) are not changed, because
* this would affect the perceived position of previously seen objects. For this reason, the worldstate should not rely
* on the head position to convert relative to absolute coordinates. Instead, it should only use transformation matrix,
* or internal conversion methods. The head position can still be used for other purposes, such as machine learning.
*
* -----------------------------------------------------------------------------------
* 0. Perform basic tasks and checks
*
* Excluded scenarios:
* 0 landmarks & <2 lines - it's a requirement from step 1
* 0 lines - step 1 allows this if there are 3 ground refs but the only marginally common
* option would be 2 feet and a corner (which is undesirable)
*
* -----------------------------------------------------------------------------------
* 1. Find the Z axis orientation vector (and add it to the preliminary transformation matrix):
* 1.1. there are >= 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 <gsl/gsl_multifit.h> //Linear least-squares fitting
#include <gsl/gsl_linalg.h> //Singular value decomposition
#include <gsl/gsl_multimin.h> //Multidimensional minimization
class LocalizerV2 {
friend class Singleton<LocalizerV2>;
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<Vector3f, 10> 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<LocalizerV2> SLocalizerV2;

15
cpp/localization/Makefile
Unescape Escape View File

@ -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

303
cpp/localization/Matrix4D.cpp
Unescape Escape View File

@ -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);
}

224
cpp/localization/Matrix4D.h
Unescape Escape View File

@ -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_

131
cpp/localization/RobovizLogger.cpp
Unescape Escape View File

@ -0,0 +1,131 @@
#include <iostream>
#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;
}

58
cpp/localization/RobovizLogger.h
Unescape Escape View File

@ -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 <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <string>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <math.h>
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_

20
cpp/localization/Singleton.h
Unescape Escape View File

@ -0,0 +1,20 @@
#ifndef SINGLETON_H
#define SINGLETON_H
template <class T>
class Singleton {
public:
static T& getInstance() {
static T instance;
return instance;
}
private:
Singleton();
~Singleton();
Singleton(Singleton const&);
Singleton& operator=(Singleton const&);
};
#endif // SINGLETON_H

197
cpp/localization/Vector3f.cpp
Unescape Escape View File

@ -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 */
}

187
cpp/localization/Vector3f.h
Unescape Escape View File

@ -0,0 +1,187 @@
#ifndef VECTOR3F_H
#define VECTOR3F_H
#include <cmath>
#include "Geometry.h"
#include <math.h>
//! 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

56
cpp/localization/World.h
Unescape Escape View File

@ -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 <vector>
#include <array>
using namespace std;
class World {
friend class Singleton<World>;
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<sLine> lines_polar;
};
typedef Singleton<World> SWorld;

196
cpp/localization/debug_main.cc
Unescape Escape View File

@ -0,0 +1,196 @@
#include <iostream>
#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<world.lines_polar.size(); i++){
cout << "Line " << i << ": " <<
world.lines_polar[i].start.x << " " <<
world.lines_polar[i].start.y << " " <<
world.lines_polar[i].start.z << " " <<
world.lines_polar[i].end.x << " " <<
world.lines_polar[i].end.y << " " <<
world.lines_polar[i].end.z << endl;
}
}
float *compute(bool lfoot_touch, bool rfoot_touch,
double feet_contact[],
bool ball_seen, double ball_pos[],
double me_pos[],
double landmarks[],
double lines[],
int lines_no){
// ================================================= 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}
world.foot_contact_rel_pos[0].x = feet_contact[0];
world.foot_contact_rel_pos[0].y = feet_contact[1];
world.foot_contact_rel_pos[0].z = feet_contact[2];
world.foot_contact_rel_pos[1].x = feet_contact[3];
world.foot_contact_rel_pos[1].y = feet_contact[4];
world.foot_contact_rel_pos[1].z = feet_contact[5];
world.ball_seen = ball_seen;
//Structure of ball_pos {ball_rel_pos_cart, ball_cheat_abs_cart_pos}
world.ball_rel_pos_cart.x = ball_pos[0];
world.ball_rel_pos_cart.y = ball_pos[1];
world.ball_rel_pos_cart.z = ball_pos[2];
world.ball_cheat_abs_cart_pos.x = ball_pos[3];
world.ball_cheat_abs_cart_pos.y = ball_pos[4];
world.ball_cheat_abs_cart_pos.z = ball_pos[5];
world.my_cheat_abs_cart_pos.x = me_pos[0];
world.my_cheat_abs_cart_pos.y = me_pos[1];
world.my_cheat_abs_cart_pos.z = me_pos[2];
for(int i=0; i<8; i++){
world.landmark[i].seen = (bool) landmarks[0];
world.landmark[i].isCorner = (bool) landmarks[1];
world.landmark[i].pos.x = landmarks[2];
world.landmark[i].pos.y = landmarks[3];
world.landmark[i].pos.z = landmarks[4];
world.landmark[i].rel_pos.x = landmarks[5];
world.landmark[i].rel_pos.y = landmarks[6];
world.landmark[i].rel_pos.z = landmarks[7];
landmarks += 8;
}
world.lines_polar.clear();
for(int i=0; i<lines_no; i++){
Vector3f s(lines[0],lines[1],lines[2]);
Vector3f e(lines[3],lines[4],lines[5]);
world.lines_polar.emplace_back(s, e);
lines += 6;
}
print_python_data();
// ================================================= 2. Compute 6D pose
loc.run();
// ================================================= 3. Prepare data to return
float retval[35];
float *ptr = retval;
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);
}
int main(){
double feet_contact[] = {0.02668597, 0.055 , -0.49031584, 0.02668597, -0.055 , -0.49031584};
double ball_pos[] = {22.3917517 , 4.91904904, -0.44419865, -0. , -0. , 0.04 };
double me_pos[] = {-22.8 , -2.44, 0.48};
double landmarks[] = { 1. , 1. , -15. , -10. , 0. , 10.88, -37.74, -2.42,
0. , 1. , -15. , 10. , 0. , 0. , 0. , 0. ,
1. , 1. , 15. , -10. , 0. , 38.56, -4.9 , -0.66,
1. , 1. , 15. , 10. , 0. , 39.75, 24.4 , -0.7 ,
1. , 0. , -15. , -1.05, 0.8 , 7.94, 16.31, 2.42,
1. , 0. , -15. , 1.05, 0.8 , 8.55, 30.15, 2.11,
1. , 0. , 15. , -1.05, 0.8 , 37.82, 8.16, 0.5 ,
1. , 0. , 15. , 1.05, 0.8 , 37.94, 11.77, 0.44 };
double lines[] = { 25.95, 35.02, -1.14, 24.02, -12.26, -1.12,
13.18, 59.93, -2.11, 10.87, -37.8 , -2.69,
39.78, 24.32, -0.75, 38.64, -5.05, -0.67,
10.89, -37.56, -2.6 , 38.52, -5.24, -0.68,
15.44, 59.85, -1.87, 39.76, 24.77, -0.88,
9.62, 3.24, -2.67, 11.02, 36.02, -2.54,
9.63, 2.82, -3.16, 7.82, 2.14, -3.67,
11.02, 36.09, -2.61, 9.51, 41.19, -2.94,
36.03, 5.33, -0.66, 36.46, 14.9 , -0.74,
35.94, 5.43, -0.72, 37.81, 5.26, -0.73,
36.42, 14.72, -0.83, 38.16, 14.68, -0.85,
20.93, 13.26, -1.33, 21.25, 9.66, -1.15,
21.21, 9.75, -1.6 , 22.18, 7.95, -1.19,
22.21, 7.94, -1.17, 23.43, 7.82, -1.11,
23.38, 7.55, -1.18, 24.42, 9.47, -1.16,
24.43, 9.37, -1.25, 24.9 , 11.72, -0.98,
24.89, 11.73, -1.2 , 24.68, 14.54, -1.05,
24.7 , 14.85, -1.06, 23.85, 16.63, -1.1 ,
23.82, 16.53, -1.14, 22.61, 17.14, -1.32,
22.65, 17.53, -1.23, 21.5 , 16.19, -1.34,
21.49, 15.92, -1.32, 20.95, 13.07, -1.32 };
int lines_no = sizeof(lines)/sizeof(lines[0])/6;
compute(true, // lfoot_touch
true, // rfoot_touch
feet_contact,
true, // ball_seen
ball_pos,
me_pos,
landmarks,
lines,
lines_no);
}

182
cpp/localization/lib_main.cpp
Unescape Escape View File

@ -0,0 +1,182 @@
#include <iostream>
#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 <pybind11/pybind11.h>
#include <pybind11/numpy.h>
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<world.lines_polar.size(); i++){
cout << "Line " << i << ": " <<
world.lines_polar[i].start.x << " " <<
world.lines_polar[i].start.y << " " <<
world.lines_polar[i].start.z << " " <<
world.lines_polar[i].end.x << " " <<
world.lines_polar[i].end.y << " " <<
world.lines_polar[i].end.z << endl;
}
}
py::array_t<float> compute(
bool lfoot_touch, bool rfoot_touch,
py::array_t<double> feet_contact,
bool ball_seen, py::array_t<double> ball_pos,
py::array_t<double> me_pos,
py::array_t<double> landmarks,
py::array_t<double> 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<lines_len; i++){
Vector3f s(lines_ptr[0],lines_ptr[1],lines_ptr[2]);
Vector3f e(lines_ptr[3],lines_ptr[4],lines_ptr[5]);
world.lines_polar.emplace_back(s, e);
lines_ptr += 6;
}
// ================================================= 2. Compute 6D pose
loc.run();
// ================================================= 3. Prepare data to return
py::array_t<float> retval = py::array_t<float>(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);
}

208
cpp/localization/robovizdraw.h
Unescape Escape View File

@ -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 <string>
#include <cstdio>
#include <cstring>
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

30
example.py
Unescape Escape View File

@ -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)

2
kill.sh
Unescape Escape View File

@ -0,0 +1,2 @@
#!/bin/bash
pkill -9 -e -f "python3 ./Run_"

Some files were not shown because too many files have changed in this diff Show More

Write Preview
Loading…
Cancel
Save