ZhaiShuaiShuai/OPEN_DUCK_MINI
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add

Browse Source
This commit is contained in:
翟帅帅 2025-07-30 11:54:39 +08:00
parent ba0513505f
commit 568ea6ab4c
186 changed files with 23227 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
pypot-master/pypot-master/.github/workflows/test_and_distribute.yml vendored Normal file
View File

@ -0,0 +1,44 @@
name: pypot
on: [push]
jobs:
build:
runs-on: ubuntu-latest
strategy:
matrix:
python-version: [3.6, 3.7, 3.8, 3.9]
env:
# The Python version from the matrix used to build and deploy to Pypi
PYTHON_BUILD_VERSION: 3.8
steps:
- uses: actions/checkout@v2
- name: Set up Python ${{ matrix.python-version }}
uses: actions/setup-python@v2
with:
python-version: ${{ matrix.python-version }}
- name: Install dependencies
run: |
python -m pip install --upgrade pip
pip install flake8 build websocket ../pypot
pip install poppy-ergo-jr
- name: Lint with flake8
run: |
# stop the build if there are Python syntax errors or undefined names
flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics
# exit-zero treats all errors as warnings. The GitHub editor is 127 chars wide
flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics
- name: Test package with unittest
working-directory: tests
run: |
python -m unittest discover
- name: Build package with Python ${{ env.PYTHON_BUILD_VERSION }}
if: ${{ github.event_name == 'push' && startsWith(github.ref, 'refs/tags') && matrix.python-version == env.PYTHON_BUILD_VERSION }}
run: python -m build
- name: Publish package relying on pypa/gh-action-pypi-publish with Python ${{ env.PYTHON_BUILD_VERSION }}
uses: pypa/gh-action-pypi-publish@27b31702a0e7fc50959f5ad993c78deac1bdfc29
if: ${{ github.event_name == 'push' && startsWith(github.ref, 'refs/tags') && matrix.python-version == env.PYTHON_BUILD_VERSION }}
with:
user: __token__
password: ${{ secrets.PYPI_API_TOKEN }}

45
pypot-master/pypot-master/.gitignore vendored Normal file
View File

@ -0,0 +1,45 @@
# Byte-compiled / optimized / DLL files
__pycache__/
*.py[cod]
*$py.class
# C extensions
*.so
# Distribution / packaging
.Python
env/
build/
develop-eggs/
dist/
downloads/
eggs/
.eggs/
lib/
lib64/
parts/
sdist/
var/
*.egg
*.egg-info
# Sphinx
doc/_build
# OS generated files
.DS_Store
.DS_Store?
.Trashes
ehthumbs.db
Thumbs.db
# Gedit
*~
# Jupyter (ipython)
.ipynb_checkpoints/
*-checkpoint.ipynb
# Pypot record files
*.record

674
pypot-master/pypot-master/LICENSE.txt Normal file
View File

@ -0,0 +1,674 @@
GNU GENERAL PUBLIC LICENSE
Version 3, 29 June 2007
Copyright (C) 2007 Free Software Foundation, Inc. <http://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 <http://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
<http://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
<http://www.gnu.org/philosophy/why-not-lgpl.html>.

4
pypot-master/pypot-master/MANIFEST.in Normal file
View File

@ -0,0 +1,4 @@
include pypot/vrep/remoteApiBindings/lib/*/*/remoteApi.*
include pypot/server/snap_projects/*
include *.md

61
pypot-master/pypot-master/README.md Normal file
View File

@ -0,0 +1,61 @@
[![PyPI](https://img.shields.io/pypi/v/pypot.svg)](https://pypi.python.org/pypi/pypot/)
[![Build Status](https://github.com/poppy-project/pypot/actions/workflows/test_and_distribute.yml/badge.svg)](https://github.com/poppy-project/pypot/actions)
[![DOI](https://zenodo.org/badge/DOI/10.5281/zenodo.591809.svg)](https://doi.org/10.5281/zenodo.591809)
# Pypot ⚙️ A Python library for Dynamixel motor control
Pypot is a cross-platform Python library making it easy and fast to control custom robots based on multiple models of Dynamixel motors. Use Pypot to:
* control Robotis motors through USB2Dynamixel, USB2AX or [Pixl 4 Raspberry Pi](https://github.com/poppy-project/pixl) devices,
* define kinematic chains of a custom robot and control it through high-level commands (Forward & Inverse Kinematics),
* define primitives (motions applying to motor groups) and easily combine them to create custom complex behaviors (Robot dance, arm shaking, writing with a pen...).
* define sensor access and processing (QRCode detection, force sensors, RGB-D, ...)
Pypot is also compatible with the [CoppeliaSim simulator](http://www.coppeliarobotics.com) (formerly V-REP), embeds a [REST API](https://docs.poppy-project.org/en/programming/rest.html) for Web-based control, and supports visual programming via [Scratch](https://docs.poppy-project.org/en/getting-started/program-the-robot.html#using-scratch) and [Snap](https://docs.poppy-project.org/en/getting-started/program-the-robot.html#using-snap).
## 🔌 Compatible hardware
**Compatible motors:** MX-106, MX-64, MX-28, MX-12, AX-12, AX-18, RX-24, RX-28, RX-64, XL-320, SR-RH4D, EX-106. Derivated versions are also supported (e.g. MX-28AT, MX-28R, MX-28T, ...). Both protocols v1 and v2 are supported but v2 is used only for XL-320. Use [Herborist](https://github.com/poppy-project/herborist#herborist) to help detect IDs and baudrates of motors.
**Compatible sensors:** Kinect 1, QRCode from RGB camera, sonar, micro-switch from Raspberry Pi GPIO, digital or analog sensor connected to Arduino
**Compatible interpreters:** Python 3.6, 3.7, 3.8, 3.9
Other models of motors and sensors can be integrated with little effort and time. Other programming languages may be connected through the REST API.
## Read 📖 [Documentation](https://docs.poppy-project.org/en/software-libraries/pypot.html) and get ⁉️ [Assistance](https://forum.poppy-project.org/)
## Pypot is part of the opensource Poppy project
Pypot is part of the [Poppy project](http://www.poppy-project.org) aiming at developing robotic creations that are easy to build, customize, deploy, and share. It promotes open-source by sharing 3D-printed hardware, software, and web tools.
The Poppy creatures are:
* **[Poppy Humanoid](https://www.poppy-project.org/en/robots/poppy-humanoid/)**: a kid-size humanoid robot designed for biped locomotion and physical human-robot interaction (25 DoF) for biped research and university workshops,
* **[Poppy Torso](https://www.poppy-project.org/en/robots/poppy-torso/)**: just the torso of the humanoid robot, with a suction pad to stick it attach it firmly to a desk (13 DoF) for HRI research, university and high school workshops
* **[Poppy Ergo Jr](https://www.poppy-project.org/en/robots/poppy-ergo-jr/)**: a low-cost robotic arm for primary to middle school (6 Dof) for primary or middle school workshops
![Poppy Humanoid](./doc/poppy-creatures.jpg)
All those creatures are based on a combination of standard dynamixel actuators, 3D printed parts and open-source electronics such as Arduino boards. Both the hardware (3D models, electronics...) and software can be freely used, modified and duplicated.
## 💻 Installation
If you are using a Poppy robot embedding a Raspberry Pi, Pypot is already shipped with it. For custom robots, just type ⌨️ `pip install pypot` in your system terminal!
If you intend to modify or add features to Pypot, create a virtual environment and install it from sources instead:
```bash
git clone https://github.com/poppy-project/pypot
cd pypot/pypot
pip install .
```
Additional drivers may be needed for USB2serial, depending of your OS. Check here:
* [USB2AX](http://www.xevelabs.com/doku.php?id=product:usb2ax:quickstart) - this device is designed to manage TTL communication only
* USB2Dynamixel - this device can manage both TTL and RS485 communication.
* [Pixl board](https://github.com/poppy-project/pixl) for RaspberryPi
## 👨‍💻 Contributing
If this is the first time you contribute to Pypot, it is a good idea to share your work on [the forum](https://forum.poppy-project.org/) first, we will be happy to give you a hand so that you can contribute to the opensource project.

58
pypot-master/pypot-master/REST-APIs.md Normal file
View File

@ -0,0 +1,58 @@
# Pypot REST APIs
The pypot library provides a REST API which can be used to access the [Robot](http://poppy-project.github.io/pypot/pypot.robot.html) level and all its attached [motors](http://poppy-project.github.io/pypot/pypot.robot.html#module-pypot.robot.motor), [sensors](http://poppy-project.github.io/pypot/pypot.robot.html#module-pypot.robot.sensor), and [primitives](http://poppy-project.github.io/pypot/pypot.primitive.html). Through the REST API, you can:
* **Motors**
* get the motors list and get/set value from/to their registers
* **Sensors**:
* get the sensors list and get/set value from/to their registers
* **Primitives**:
* get the primitives list (running or not), start, stop, pause, and resume them.
* you can also access their publicly available properties and methods.
*Note that only the defined as **publicly available** registers or methods will be accessed through the REST API. Please refer to the [note for developers](#markdown-header-note-for-developers) below for details.*
# REST APIs
*Please note that all answers are always sent as json dictionary.*
## Robot
### Motor
| | HTTP | JSON | Example of answer |
|--------------------------------------------|:------------------------------------------------------------:|:---------------------------------------------------------------------------------------------------------------------------------------------------:|------------------------------------------------------------------------|
| Get the motors list | GET /motor/list.json | {"robot": {"get_motors_list": {"alias": "motors"}}} | {'motors': ["l_elbow_y", "r_elbow_y", "r_knee_y", "head_y", "head_z"]} |
| Get the motors alias list | GET /motor/alias/list.json | {"robot": {"get_motors_alias": {}}} | {'alias': ["r_leg", "torso", "l_leg_sagitall"]} |
| Get the motors list of a specific alias | GET /motor/\<alias>/list.json | {"robot": {"get_motors_list": {"alias": "<alias>"}}} | {\<alias>: ["l_elbow_y", "r_elbow_y", "r_knee_y", "head_y", "head_z"]} |
| Get the registers list of a specific motor | GET /motor/\<motor_name>/register/list.json | {"robot": {"get_registers_list": {"motor": "<motor_name>"}}} | {'registers': ["goal_speed", "compliant", "present_load", "id"]} |
| Get the register value | GET /motor/\<motor_name>/register/\<register_name> | {"robot": {"get_register_value": {"motor": "<motor_name>", "register": "<register_name>"}}} | {"present_position": 30} |
| Set new value to a register | POST /motor/\<motor_name>/register/\<register_name>/value.json | {"robot": {"set_register_value": {"motor": "<motor_name>", "register": "<register_name>", "value": {"arg1": "val1", "arg2": "val2", "...": "..."}}} | {} |
### Sensor
*Similar to the motor API. You just replace motor by sensor (for the moment there is no alias for sensors).*
## Primitive
| | HTTP | JSON | Example of answer |
|-----------------------------------|:-------------------------------------------------:|:--------------------------------------------------------------------------------------------------------------------------------------------:|:----------------------------------------------------------------------:|
| Get the primitives list | GET /primitive/list.json | {"robot": {"get_primitives_list": ""}} | {'primitives': ["stand_up", "sit", "head_tracking"]} |
| Get the running primitives list | GET /primitive/running/list.json | {"robot": {"get_running_primitives_list": ""}} | {'primitives': ["head_tracking"]} |
| Start a primitive | GET /primitive/\<prim>/start.json | {"robot": {"start_primitive": {"primitive": "<prim>"}}} | {} |
| Stop a primitive | GET /primitive/\<prim>/stop.json | {"robot": {"stop_primitive": {"primitive": "<prim>"}}} | {} |
| Pause a primitive | GET /primitive/\<prim>/pause.json | {"robot": {"pause_primitive": {"primitive": "<prim>"}}} | {} |
| Resume a primitive | GET /primitive/\<prim>/resume.json | {"robot": {"resume_primitive": {"primitive": "<prim>"}}} | {} |
| Get the primitive properties list | GET /primitive/\<prim>/property/list.json | {"robot": {"get_primitive_properties_list": {"primitive": "<prim>"}}} | {"property": ["filter", "smooth"]} |
| Get a primitive property value | GET /primitive/\<prim>/property/<prop> | {"robot": {"get_primitive_property": {"primitive": "<prim>", "property": "<prop>"}}} | {"sin.amp": 30.0} |
| Set a primitive property value | POST /primitive/\<prim>/property/<prop>/value.json | {"robot": {"set_primitive_property": {"primitive": "<prim>", "property": "<prop>", "args": {"arg1": "val1", "arg2": "val2", "...": "..."}}}} | {} |
| Get the primitive methods list | GET /primitive/\<prim>/method/list.json | {"robot": {"get_primitive_methods_list": {"primitive": "<prim>"}}} | {"methods": ["get_tracked_faces", "start", "stop", "pause", "resume"]} |
| Call a method of a primitive | POST /primitive/\<prim>/method/\<meth>/args.json | {"robot": {"call_primitive_method": {"primitive": "<prim>", "method": "<meth>", "args": {"arg1": "val1", "arg2": "val2", "...": "..."}}}} | |
## Note for developers
In order to **publicly** available through the REST API, the registers of the motors/sensors and the properties/methods of the primitives should be added to specific lists.
More precisely, the [Motor](http://poppy-project.github.io/pypot/pypot.robot.html#pypot.robot.motor.Motor) class sets the [registers](http://poppy-project.github.io/pypot/pypot.dynamixel.html#pypot.dynamixel.motor.DxlMotor.registers) list (similarly for the [Sensor](http://poppy-project.github.io/pypot/pypot.sensor.html) class) and the [Primitives](http://poppy-project.github.io/pypot/pypot.primitive.html#pypot.primitive.primitive.Primitive) uses the [methods]() and [properties]() list.
Those are class variables and can be extended when defining your own subclasses (see the [Sinus primitive](https://github.com/poppy-project/pypot/blob/REST-API-2.0/pypot/primitive/utils.py) as an example).

106
pypot-master/pypot-master/changelog.md Normal file
View File

@ -0,0 +1,106 @@
# Changelog
## V 4.1
* Add support for EX motors
## V 4.0
* Fix compatibility with Python 3.8 and drop compatibility with Python 2
* Fix poppy-configure and dxl-config for recent Dynamixel firmwares with protocol v1
* Replaced the Snap build from pypot by a Snap downloader for `poppy-services --snap`
* Add Snap entry points for webcam access in the REST API
* Jupyter notebooks update for py3
* Updated ikpy to the latest version
## V 3.1
* Add support for connecting multiple robots to a same VREP scene.
* Fix a bug in the minjerk trajectory computation.
## V 2.11
## Features
* Add dummy motors (mostly for unittest)
* add native support for the pixl board
* allow to disable sensor at loading (convenient for camera sensor)
* add a dummy camera
* Add support for RX-24 dynamixel motors
* Add an event used to check if a "loopable" thread has been updated
* Move can now be plotted using matplotlib
### Snap
* Add blocs: “ping url <hostname>” and “set $robot host to <hostname>” which aim to fix DNS issues in some filtered networks.
* update “set <register> of motor(s) <motors> to value <value>” : speed register is now moving_speed instead of goal_speed. Able to use it through many motors at once now
* fix “get <register> of motor(s) <motors>”
* fix some default values of inputs variable for consistency
* add entry for ik in SnapRemoteServer
* check return-delay-time at startup to prevent timeouts with misconfigured motors
## Bugfix
* many primitives threading issues
* python >= 3.4 compatibility issues
* setup unittest via dummy robot
* fix the unclear exception "Cannot unpack *values"
* fix cli tool `poppy-motor-reset` and rename it to `dxl-config`
* Fix deprecation issue in get_control_table
* Clear error when there is no "time script" in a v-rep scene
* Fix a freeze when stopping a paused primitive
* Fix offset/orientation issue in DummyController
* Fix hostname resolution
* Make initialization of synchronization loop more robust
* Fix #155: Closing a DummyRobot raises an issue
## V 2.10
### Features
* add support for led inside primitive (XL320)
* remove RPICam and use v4l driver in opencv
* support hampy marker in Snap!
### Fix
* network issue for finding local ip when there is no interface
* Python 3.5 compatibility
## V 2.9
### Image feature Sensors
* face
* blob
* qrcode
### LED register for XL-320 motors
### various bug fixes
## V 2.8
### Sync Loop
* possibility to define synchronous loop
* define a "light" synchronization loop
* can now choose sync. loop inside the config
### Better Sensor Integration
* can specify sensor inside the config file
## V 2.1
* now uses the poppy_creature package
* add support for present_load/torque_limit/compliant in V-REP
* fix a bug when using setup.py install
* add minimum jerk of Steve N'Guyen
* add safe compliance behavior
* add camera sensor based on opencv
## V. 2.0
### Major changes
* support for V-REP simulator
* new controller implementation: [extending pypot](http://poppy-project.github.io/pypot/extending.html)
### Minor changes
* Use of descriptors for motor registers
* REST API / remote robot
* Starts automatically the synchronization
## V. 1.7
### Minor changes
* Autodetect robot

BIN
pypot-master/pypot-master/doc/ErgoRobots.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 30 KiB

12
pypot-master/pypot-master/doc/FAQ.rst Normal file
View File

@ -0,0 +1,12 @@
FAQ
***
Why is the default baud rate different for robotis and for pypot ?
------------------------------------------------------------------
Robotis motors are set up to work with a 57140 baud rate. Yet, in pypot we choose to use 1000000 baud rate as the default configuration. While everything would work with the robotis default baud rate, we choose to incitate people to modify this default configuration to allow for more performance.
I got a DxlCommunicationError when scanning multiple motors on a bus
--------------------------------------------------------------------
This exception is usually raised when two (or more) motors share the same id. This should never happened, all ids should be unique on a same bus. Otherwise, package will collide resulting in communication error.

153
pypot-master/pypot-master/doc/Makefile Normal file
View File

@ -0,0 +1,153 @@
# Makefile for Sphinx documentation
#
# You can set these variables from the command line.
SPHINXOPTS =
SPHINXBUILD = sphinx-build
PAPER =
BUILDDIR = _build
# Internal variables.
PAPEROPT_a4 = -D latex_paper_size=a4
PAPEROPT_letter = -D latex_paper_size=letter
ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) .
# the i18n builder cannot share the environment and doctrees with the others
I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) .
.PHONY: help clean html dirhtml singlehtml pickle json htmlhelp qthelp devhelp epub latex latexpdf text man changes linkcheck doctest gettext
help:
@echo "Please use \`make <target>' where <target> is one of"
@echo " html to make standalone HTML files"
@echo " dirhtml to make HTML files named index.html in directories"
@echo " singlehtml to make a single large HTML file"
@echo " pickle to make pickle files"
@echo " json to make JSON files"
@echo " htmlhelp to make HTML files and a HTML help project"
@echo " qthelp to make HTML files and a qthelp project"
@echo " devhelp to make HTML files and a Devhelp project"
@echo " epub to make an epub"
@echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter"
@echo " latexpdf to make LaTeX files and run them through pdflatex"
@echo " text to make text files"
@echo " man to make manual pages"
@echo " texinfo to make Texinfo files"
@echo " info to make Texinfo files and run them through makeinfo"
@echo " gettext to make PO message catalogs"
@echo " changes to make an overview of all changed/added/deprecated items"
@echo " linkcheck to check all external links for integrity"
@echo " doctest to run all doctests embedded in the documentation (if enabled)"
clean:
-rm -rf $(BUILDDIR)/*
html:
$(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html
@echo
@echo "Build finished. The HTML pages are in $(BUILDDIR)/html."
dirhtml:
$(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml
@echo
@echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml."
singlehtml:
$(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml
@echo
@echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml."
pickle:
$(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle
@echo
@echo "Build finished; now you can process the pickle files."
json:
$(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json
@echo
@echo "Build finished; now you can process the JSON files."
htmlhelp:
$(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp
@echo
@echo "Build finished; now you can run HTML Help Workshop with the" \
".hhp project file in $(BUILDDIR)/htmlhelp."
qthelp:
$(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp
@echo
@echo "Build finished; now you can run "qcollectiongenerator" with the" \
".qhcp project file in $(BUILDDIR)/qthelp, like this:"
@echo "# qcollectiongenerator $(BUILDDIR)/qthelp/PyPot.qhcp"
@echo "To view the help file:"
@echo "# assistant -collectionFile $(BUILDDIR)/qthelp/PyPot.qhc"
devhelp:
$(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp
@echo
@echo "Build finished."
@echo "To view the help file:"
@echo "# mkdir -p $$HOME/.local/share/devhelp/PyPot"
@echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/PyPot"
@echo "# devhelp"
epub:
$(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub
@echo
@echo "Build finished. The epub file is in $(BUILDDIR)/epub."
latex:
$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex
@echo
@echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex."
@echo "Run \`make' in that directory to run these through (pdf)latex" \
"(use \`make latexpdf' here to do that automatically)."
latexpdf:
$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex
@echo "Running LaTeX files through pdflatex..."
$(MAKE) -C $(BUILDDIR)/latex all-pdf
@echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex."
text:
$(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text
@echo
@echo "Build finished. The text files are in $(BUILDDIR)/text."
man:
$(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man
@echo
@echo "Build finished. The manual pages are in $(BUILDDIR)/man."
texinfo:
$(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo
@echo
@echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo."
@echo "Run \`make' in that directory to run these through makeinfo" \
"(use \`make info' here to do that automatically)."
info:
$(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo
@echo "Running Texinfo files through makeinfo..."
make -C $(BUILDDIR)/texinfo info
@echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo."
gettext:
$(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale
@echo
@echo "Build finished. The message catalogs are in $(BUILDDIR)/locale."
changes:
$(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes
@echo
@echo "The overview file is in $(BUILDDIR)/changes."
linkcheck:
$(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck
@echo
@echo "Link check complete; look for any errors in the above output " \
"or in $(BUILDDIR)/linkcheck/output.txt."
doctest:
$(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest
@echo "Testing of doctests in the sources finished, look at the " \
"results in $(BUILDDIR)/doctest/output.txt."

22
pypot-master/pypot-master/doc/README.md Normal file
View File

@ -0,0 +1,22 @@
# Building docs #
To build docs, run `make` in this directory. `make help` lists all targets.
## Requirements ##
Sphinx and Latex is needed to build doc.
**Spinx:**
```sh
pip install sphinx sphinxjp.themes.basicstrap
```
**Latex Ubuntu:**
```sh
sudo apt-get install -qq texlive texlive-latex-extra dvipng
```
**Latex Mac:**
Install the full [MacTex](http://www.tug.org/mactex/) installation or install the smaller [BasicTex](http://www.tug.org/mactex/morepackages.html) and add *ucs* and *dvipng* packages:
```sh
sudo tlmgr install ucs dvipng
```

0
pypot-master/pypot-master/doc/_static/my-css.css vendored Normal file
View File

5
pypot-master/pypot-master/doc/_templates/layout.html vendored Normal file
View File

@ -0,0 +1,5 @@
{# Import the theme's layout #}
{% extends "!layout.html" %}
{% set css_files = ['_static/my-css.css'] + css_files %}

22
pypot-master/pypot-master/doc/about.rst Normal file
View File

@ -0,0 +1,22 @@
What is pypot?
==============
.. image:: banderole-pypot.jpg
:width: 100%
:align: center
Pypot is a framework developed in the `Inria FLOWERS <https://flowers.inria.fr/>`_ team to make it easy and fast to control custom robots based on dynamixel motors. This framework provides different level of abstraction corresponding to different types of use. More precisely, you can use pypot to:
* directly control robotis motors through a USB2serial device (both protocols v1 and v2 are supported: you can use it with AX, RX, MX and XL320 motors),
* define the structure of your particular robot and control it through high-level commands.
.. * define primitives and easily combine them to create complex behavior.
Pypot has been entirely written in Python to allow for fast development, easy deployment and quick scripting by non-necessary expert developers. The serial communication is handled through the standard library and thus allows for rather high performance (10ms sensorimotor loop). It is crossed-platform and has been tested on Linux, Windows and Mac OS. It is distributed under the `GPL V3 open source license <http://www.gnu.org/copyleft/gpl.html>`_.
Pypot is also compatible with the `V-REP simulator <http://www.coppeliarobotics.com>`_. This allows you to seamlessly switch from a real robot to its simulated equivalent without having to modify your code.
The next sections describe how to :ref:`install <installation>` pypot on your system and then the :ref:`first steps to control an Ergo-Robot <quickstart>`. If you decide to use pypot and want more details on what you can do with this framework, you can refer to the :ref:`tutorial <tutorial>`.
.. note:: Pypot is part of the `poppy-project <http://www.poppy-project.org>`_ and is mainly used to control Poppy Creatures. If you are not interested in the low-level communication with motors and sensors but rather on high-level behaviors of Poppy Robots, you should directly see the poppy-* libraries (e.g. `Poppy Humanoid <https://github.com/poppy-project/poppy-humanoid>`_ or `Poppy Ergo Jr <https://github.com/poppy-project/poppy-ergo-jr>`_). They are built on top of pypot and abstract most of its operating and already come with convenient method for creating and starting your robot. They are also a good starting point if you want to define your own Poppy Creatures.

12
pypot-master/pypot-master/doc/api.rst Normal file
View File

@ -0,0 +1,12 @@
Pypot's API
***********
.. toctree::
pypot.dynamixel
pypot.primitive
pypot.robot
pypot.sensor
pypot.server
pypot.vrep
pypot.utils

BIN
pypot-master/pypot-master/doc/banderole-pypot.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 45 KiB

257
pypot-master/pypot-master/doc/conf.py Normal file
View File

@ -0,0 +1,257 @@
# -*- coding: utf-8 -*-
#
# Pypot documentation build configuration file, created by
# sphinx-quickstart on Sat Sep 15 18:52:00 2012.
#
# This file is execfile()d with the current directory set to its containing dir.
#
# Note that not all possible configuration values are present in this
# autogenerated file.
#
# All configuration values have a default; values that are commented out
# serve to show the default.
import sys
import os
import re
# If extensions (or modules to document with autodoc) are in another directory,
# add these directories to sys.path here. If the directory is relative to the
# documentation root, use os.path.abspath to make it absolute, like shown here.
# sys.path.insert(0, os.path.abspath('..'))
# -- General configuration -----------------------------------------------------
# If your documentation needs a minimal Sphinx version, state it here.
#needs_sphinx = '1.0'
# Add any Sphinx extension module names here, as strings. They can be extensions
# coming with Sphinx (named 'sphinx.ext.*') or your custom ones.
extensions = ['sphinx.ext.autodoc',
'sphinx.ext.intersphinx',
'sphinx.ext.viewcode',
'sphinx.ext.coverage',
'sphinxjp.themes.basicstrap']
# Add any paths that contain templates here, relative to this directory.
templates_path = ['_templates']
# The suffix of source filenames.
source_suffix = '.rst'
# The encoding of source files.
#source_encoding = 'utf-8-sig'
# The master toctree document.
master_doc = 'index'
# General information about the project.
project = 'pypot'
copyright = '2015, Poppy-Project'
# The version info for the project you're documenting, acts as replacement for
# |version| and |release|, also used in various other places throughout the
# built documents.
def version():
with open('../pypot/_version.py') as f:
return re.search(r"^__version__ = ['\"]([^'\"]*)['\"]", f.read()).group(1)
version = release = version()
# The language for content autogenerated by Sphinx. Refer to documentation
# for a list of supported languages.
#language = None
# There are two options for replacing |today|: either, you set today to some
# non-false value, then it is used:
#today = ''
# Else, today_fmt is used as the format for a strftime call.
#today_fmt = '%B %d, %Y'
# List of patterns, relative to source directory, that match files and
# directories to ignore when looking for source files.
exclude_patterns = ['_build']
# The reST default role (used for this markup: `text`) to use for all documents.
#default_role = None
# If true, '()' will be appended to :func: etc. cross-reference text.
#add_function_parentheses = True
# If true, the current module name will be prepended to all description
# unit titles (such as .. function::).
#add_module_names = True
# If true, sectionauthor and moduleauthor directives will be shown in the
# output. They are ignored by default.
#show_authors = False
# The name of the Pygments (syntax highlighting) style to use.
pygments_style = 'sphinx'
# A list of ignored prefixes for module index sorting.
#modindex_common_prefix = []
autoclass_content = 'both'
autodoc_member_order = 'bysource'
# -- Options for HTML output ---------------------------------------------------
# The theme to use for HTML and HTML Help pages. See the documentation for
# a list of builtin themes.
#html_theme = 'sphinxdoc'
html_theme = 'basicstrap'
# Theme options are theme-specific and customize the look and feel of a theme
# further. For a list of options available for each theme, see the
# documentation.
# html_theme_options = {}
# Add any paths that contain custom themes here, relative to this directory.
#html_theme_path = []
# The name for this set of Sphinx documents. If None, it defaults to
# "<project> v<release> documentation".
#html_title = None
# A shorter title for the navigation bar. Default is the same as html_title.
#html_short_title = None
# The name of an image file (relative to this directory) to place at the top
# of the sidebar.
html_logo = 'pypot_logo-144x144.png'
# The name of an image file (within the static path) to use as favicon of the
# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32
# pixels large.
html_favicon = 'pypot_logo-48x48.png'
# Add any paths that contain custom static files (such as style sheets) here,
# relative to this directory. They are copied after the builtin static files,
# so a file named "default.css" will overwrite the builtin "default.css".
html_static_path = ['_static']
# If not '', a 'Last updated on:' timestamp is inserted at every page bottom,
# using the given strftime format.
#html_last_updated_fmt = '%b %d, %Y'
# If true, SmartyPants will be used to convert quotes and dashes to
# typographically correct entities.
#html_use_smartypants = True
# Custom sidebar templates, maps document names to template names.
#html_sidebars = {}
# Additional templates that should be rendered to pages, maps page names to
# template names.
#html_additional_pages = {}
# If false, no module index is generated.
#html_domain_indices = True
# If false, no index is generated.
#html_use_index = True
# If true, the index is split into individual pages for each letter.
#html_split_index = False
# If true, links to the reST sources are added to the pages.
#html_show_sourcelink = True
# If true, "Created using Sphinx" is shown in the HTML footer. Default is True.
#html_show_sphinx = True
# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True.
#html_show_copyright = True
# If true, an OpenSearch description file will be output, and all pages will
# contain a <link> tag referring to it. The value of this option must be the
# base URL from which the finished HTML is served.
#html_use_opensearch = ''
# This is the file name suffix for HTML files (e.g. ".xhtml").
#html_file_suffix = None
# Output file base name for HTML help builder.
htmlhelp_basename = 'Pypotdoc'
# -- Options for LaTeX output --------------------------------------------------
latex_elements = {
# The paper size ('letterpaper' or 'a4paper').
#'papersize': 'letterpaper',
# The font size ('10pt', '11pt' or '12pt').
#'pointsize': '10pt',
# Additional stuff for the LaTeX preamble.
#'preamble': '',
}
# Grouping the document tree into LaTeX files. List of tuples
# (source start file, target name, title, author, documentclass [howto/manual]).
latex_documents = [
('index', 'Pypot.tex', 'Pypot Documentation',
'Poppy-Project', 'manual'),
]
# The name of an image file (relative to this directory) to place at the top of
# the title page.
latex_logo = 'pypot_logo.png'
# For "manual" documents, if this is true, then toplevel headings are parts,
# not chapters.
#latex_use_parts = False
# If true, show page references after internal links.
#latex_show_pagerefs = False
# If true, show URL addresses after external links.
#latex_show_urls = False
# Documents to append as an appendix to all manuals.
#latex_appendices = []
# If false, no module index is generated.
#latex_domain_indices = True
# -- Options for manual page output --------------------------------------------
# One entry per manual page. List of tuples
# (source start file, name, description, authors, manual section).
man_pages = [
('index', 'pypot', 'Pypot Documentation',
['Poppy-Project'], 1)
]
# If true, show URL addresses after external links.
#man_show_urls = False
# -- Options for Texinfo output ------------------------------------------------
# Grouping the document tree into Texinfo files. List of tuples
# (source start file, target name, title, author,
# dir menu entry, description, category)
texinfo_documents = [
('index', 'pypot', 'Pypot Documentation',
'Poppy-Project', 'Pypot', 'One line description of project.',
'Miscellaneous'),
]
# Documents to append as an appendix to all manuals.
#texinfo_appendices = []
# If false, no module index is generated.
#texinfo_domain_indices = True
# How to display URL addresses: 'footnote', 'no', or 'inline'.
#texinfo_show_urls = 'footnote'
# Example configuration for intersphinx: refer to the Python standard library.
intersphinx_mapping = {'http://docs.python.org/': None}

335
pypot-master/pypot-master/doc/controller.rst Normal file
View File

@ -0,0 +1,335 @@
.. _controller:
Robot Controller
================
Using the robot abstraction
---------------------------
While the :ref:`low_level` provides access to all functionalities of the dynamixel motors, it forces you to have synchronous calls which can take a non-negligible amount of time. In particular, most programs will need to have a really fast read/write synchronization loop, where we typically read all motor position, speed, load and set new values, while in parallel we would like to have higher level code that computes those new values. This is pretty much what the robot abstraction is doing for you. More precisely, through the use of the class :class:`~pypot.robot.robot.Robot` you can:
* automatically initialize all connections (make transparent the use of multiple USB2serial connections),
* define :attr:`~pypot.dynamixel.motor.DxlMotor.offset` and :attr:`~pypot.dynamixel.motor.DxlMotor.direct` attributes for motors,
* automatically define accessor for motors and their most frequently used registers (such as :attr:`~pypot.dynamixel.motor.DxlMotor.goal_position`, :attr:`~pypot.dynamixel.motor.DxlMotor.present_speed`, :attr:`~pypot.dynamixel.motor.DxlMotor.present_load`, :attr:`~pypot.dynamixel.motor.DxlMXMotor.pid`, :attr:`~pypot.dynamixel.motor.DxlMotor.compliant`),
* define read/write synchronization loop that will run in background.
We will first see how to define your robot thanks to the writing of a :ref:`configuration <config_file>`, then we will describe how to set up :ref:`synchronization loops <sync_loop>`. Finally, we will show how to easily :ref:`control this robot through asynchronous commands <control_robot>`.
.. _config_file:
Writing the configuration
-------------------------
The configuration, described as a Python dictionary, contains several important features that help build both your robot and the software to manage you robot. The important fields are listed below:
* **controllers** - This key holds the information pertaining to a controller and all the items connected to its bus.
* **motors** - This is a description of all the custom setup values for each motor. Meta information, such as the motor access name or orientation, is also included here. It is also there that you will set the angle limits of the motor.
* **motorgroups** - This is used to define alias of a group of motors (e.g. left_leg).
.. note:: The configuration can be written programmatically or can be loaded from any file that can be loaded as a dict (e.g. a JSON file).
Now let's detail each section. To better understand how the configuration is structure it is probably easier to start from one of the example provided with pypot and modify it (e.g. :obj:`pypot.robot.config.ergo_robot_config`):
#. **controllers**: You can have a single or multiple :class:`~pypot.dynamixel.controller.DxlController`. For each of them, you should indicate whether or not to use the SYNC_READ instruction (only the USB2AX device currently supported it). When you describe your controller, you must also include the port that the device is connected to (see :ref:`open_connection`). In this section, you can also specify which robotis protocol to use (if not specified it uses the v1). You also have to specify which motors are attached to this bus. You can either give individual motors or groups (see the sections below)::
my_config['controllers'] = {}
my_config['controllers']['upper_body_controler'] = {
'port': '/dev/ttyUSB0',
'sync_read': False,
'attached_motors': ['torso', 'head', 'arms'],
'protocol': 1,
}
#. **motorgroups**: Here, you can define the different motors group corresponding to the structure of your robot. It will automatically create an alias for the group. Groups can be nested, i.e. a group can be included inside another group, as in the example below::
my_config['motorgroups'] = {
'torso': ['arms', 'head_x', 'head_y'],
'arms': ['left_arm', 'right_arm'],
'left_arm': ['l_shoulder_x', 'l_shoulder_y', 'l_elbow'],
'right_arm': ['r_shoulder_x', 'r_shoulder_y', 'r_elbow']
}
#. **motors**: Then, you add all the motors. The attributes are not optional and describe how the motors can be used in the software. You have to specify the type of motor, it will change which attributes are available (e.g. compliance margin versus pid gains). The name and id are used to access the motor specifically. Orientation describes whether the motor will act in an anti-clockwise fashion (direct) or clockwise (indirect). You should also provide the angle limits of your motor. They will be checked automatically at every start up and changed if needed::
my_config['motors'] = {}
my_config['motors']['l_hip_y'] = {
'id': 11,
'type': 'MX-28',
'orientation': 'direct',
'offset': 0.0,
'angle_limit': (-90.0, 90.0),
}
#. This is all you need to create and interact with your robot. All that remains is to connect your robot to your computer. To create your robot use the :func:`~pypot.robot.config.from_config` function which takes your configuration as an argument. Here is an example of how to create your first robot and start using it::
import pypot.robot
robot = pypot.robot.from_config(my_config)
for m in robot.left_arm:
print(m.present_position)
#. (optional) If you prefer working with file, you can read/write your config to any format that can be transformed into a dictionary. For instance, you can easily use the JSON format::
import json
import pypot.robot
from pypot.robot.config import ergo_robot_config
with open('ergo.json', 'w') as f:
json.dump(ergo_robot_config, f, indent=2)
ergo = pypot.robot.from_json('ergo.json')
To give you a complete overview of what your config should look like, here is the listing of the Ergo-Robot config dictionary::
ergo_robot_config = {
'controllers': {
'my_dxl_controller': {
'sync_read': False,
'attached_motors': ['base', 'tip'],
'port': 'auto'
}
},
'motorgroups': {
'base': ['m1', 'm2', 'm3'],
'tip': ['m4', 'm5', 'm6']
},
'motors': {
'm5': {
'orientation': 'indirect',
'type': 'MX-28',
'id': 15,
'angle_limit': [-90.0, 90.0],
'offset': 0.0
},
'm4': {
'orientation': 'direct',
'type': 'MX-28',
'id': 14,
'angle_limit': [-90.0, 90.0],
'offset': 0.0
},
'm6': {
'orientation': 'indirect',
'type': 'MX-28',
'id': 16,
'angle_limit': [-90.0, 90.0],
'offset': 0.0
},
'm1': {
'orientation': 'direct',
'type': 'MX-28', 'id': 11,
'angle_limit': [-90.0, 90.0],
'offset': 0.0
},
'm3': {
'orientation': 'indirect',
'type': 'MX-28',
'id': 13,
'angle_limit': [-90.0, 90.0],
'offset': 0.0
},
'm2': {
'orientation': 'indirect',
'type': 'MX-28',
'id': 12,
'angle_limit': [-90.0, 90.0],
'offset': 0.0
}
}
}
Since pypot 1.7, you can now set the port to 'auto' in the dictionary. When loading the configuration, pypot will automatically try to find the port with the corresponding attached motor ids.
.. note:: While this is convenient as the same config file can be use on multiple machine, it also slows the creation of the :class:`~pypot.robot.robot.Robot`.
Auto-detection and generation of the configuration
--------------------------------------------------
Pypot provides another way of creating your :class:`~pypot.robot.robot.Robot`. The :func:`~pypot.dynamixel.autodetect_robot` can scan all dynamixel ports plugged and find all connected motors. It then returns the corresponding :class:`~pypot.robot.robot.Robot`. For instance::
from pypot.dynamixel import autodetect_robot
my_robot = autodetect_robot()
for m in my_robot.motors:
m.goal_position = 0.0
.. note:: As the :func:`~pypot.dynamixel.autodetect_robot` function scans all available ports, it can be quite slow (few seconds). So this should be used to first discover the robot configuration and then export it (see below).
If you have manually created your :class:`~pypot.robot.robot.Robot` (or thanks to the :func:`~pypot.dynamixel.autodetect_robot` function), you can then use the :meth:`~pypot.robot.Robot.to_config` method to export the :class:`~pypot.robot.robot.Robot` current configuration.
This configuration can then be easily saved::
import json
config = my_robot.to_config()
with open('my_robot.json', 'wb') as f:
json.dump(config, f)
You can then easily re-create your robot::
from pypot.robot import from_json
my_robot = from_json('my_robot.json')
.. _sync_loop:
Dynamixel controller and Synchronization Loop
---------------------------------------------
As indicated above, the :class:`~pypot.robot.robot.Robot` held instances of :class:`~pypot.dynamixel.motor.DxlMotor`. Each of this instance represents a real motor of your physical robot. The attributes of those "software" motors are automatically synchronized with the real "hardware" motors. In order to do that, the :class:`~pypot.robot.robot.Robot` class uses a :class:`~pypot.dynamixel.controller.DxlController` which defines synchronization loops that will read/write the registers of dynamixel motors at a predefined frequency.
.. warning:: The synchronization loops will try to run at the defined frequency, however don't forget that you are limited by the bus bandwidth! For instance, depending on your robot you will not be able to read/write the position of all motors at 100Hz. Moreover, the loops are implemented as python thread and we can thus not guarantee the exact frequency of the loop.
If you looked closely at the example above, you could have noticed that even without defining any controller nor synchronization loop, you can already read the present position of the motors. Indeed, by default the class :class:`~pypot.robot.robot.Robot` uses a particular controller :class:`~pypot.dynamixel.controller.BaseDxlController` which already defines synchronization loops. More precisely, this controller:
* reads the present position, speed, load at 50Hz,
* writes the goal position, moving speed and torque limit at 50Hz,
* writes the pid or compliance margin/slope (depending on the type of motor) at 10Hz,
* reads the present temperature and voltage at 1Hz.
So, in most case you should not have to worry about synchronization loop and it should directly work. Off course, if you want to synchronize other values than the ones listed above you will have to modify this default behavior.
.. note:: With the current version of pypot, you can not indicate in the configuration which subclasses of :class:`~pypot.dynamixel.controller.DxlController` you want to use. This feature should be added in a future version. If you want to use your own controller, you should either modify the config parser, modify the :class:`~pypot.dynamixel.controller.BaseDxlController` class or directly instantiate the :class:`~pypot.robot.robot.Robot` class.
The synchronization loops are automatically started when instantiating your robot, the method :meth:`~pypot.robot.robot.Robot.start_sync` is directly called. You can also stop the synchronization if needed (see the :meth:`~pypot.robot.robot.Robot.stop_sync` method). Note that prior to version 2, the synchronization is not started by default.
.. warning:: You should never set values to motors when the synchronization is not running.
Now you have a robot that is reading and writing values to each motor in an infinite loop. Whenever you access these values, you are accessing only their most recent versions that have been read at the frequency of the loop. This automatically make the synchronization loop run in background. You do not need to wait the answer of a read command to access data (this can take some time) so that algorithms with heavy computation do not encounter a bottleneck when values from motors must be known.
Now you are ready to create some behaviors for your robot.
.. _control_robot:
Controlling your robot
----------------------
Controlling in position
+++++++++++++++++++++++
As shown in the examples above, the robot class let you directly access the different motors. For instance, let's assume we are working with an Ergo-robot, you could then write::
import pypot.robot
from pypot.robot.config import ergo_robot_config
robot = pypot.robot.from_config(ergo_robot_config)
# Note that all these calls will return immediately,
# and the orders will not be directly sent
# (they will be sent during the next write loop iteration).
for m in ergo_robot.base:
m.compliant = False
m.goal_position = 0
# This will return the last synchronized value
print(ergo_robot.base_pan.present_position)
For a complete list of all the attributes that you can access, you should refer to the :class:`~pypot.dynamixel.motor.DxlMotor` API.
As an example of what you can easily do with the Robot API, we are going to write a simple program that will make a robot with two motors move with sinusoidal motions. More precisely, we will apply a sinusoid to one motor and the other one will read the value of the first motor and use it as its own goal position. We will still use an Ergo-robot as example::
import time
import numpy
import pypot.robot
from pypot.robot.config import ergo_robot_config
amp = 30
freq = 0.5
robot = pypot.robot.from_config(ergo_robot_config)
# Put the robot in its initial position
for m in ergo_robot.motors: # Note that we always provide an alias for all motors.
m.compliant = False
m.goal_position = 0
# Wait for the robot to actually reach the base position.
time.sleep(2)
# Do the sinusoidal motions for 10 seconds
t0 = time.time()
while True:
t = time.time() - t0
if t > 10:
break
pos = amp * numpy.sin(2 * numpy.pi * freq * t)
ergo_robot.base_pan.goal_position = pos
# In order to make the other sinus more visible,
# we apply it with an opposite phase and we increase the amplitude.
ergo_robot.head_pan.goal_position = -1.5 * ergo_robot.base_pan.present_position
# We want to run this loop at 50Hz.
time.sleep(0.02)
Controlling in speed
++++++++++++++++++++
Thanks to the :attr:`~pypot.dynamixel.motor.DxlMotor.goal_speed` property you can also control your robot in speed. More precisely, by setting :attr:`~pypot.dynamixel.motor.DxlMotor.goal_speed` you will change the :attr:`~pypot.dynamixel.motor.DxlMotor.moving_speed` of your motor but you will also automatically change the :attr:`~pypot.dynamixel.motor.DxlMotor.goal_position` that will be set to the angle limit in the desired direction.
.. note:: You could also use the wheel mode settings where you can directly change the :attr:`~pypot.dynamixel.motor.DxlMotor.moving_speed`. Nevertheless, while the motor will turn infinitely with the wheel mode, here with the :attr:`~pypot.dynamixel.motor.DxlMotor.goal_speed` the motor will still respect the angle limits.
As an example, you could write::
t = numpy.arange(0, 10, 0.01)
speeds = amp * numpy.cos(2 * numpy.pi * freq * t)
positions = []
for s in speeds:
ergo_robot.head_pan.goal_speed = s
positions.append(ergo_robot.head_pan.present_position)
time.sleep(0.05)
# By applying a cosinus on the speed
# You observe a sinusoid on the position
plot(positions)
.. warning:: If you set both :attr:`~pypot.dynamixel.motor.DxlMotor.goal_speed` and :attr:`~pypot.dynamixel.motor.DxlMotor.goal_position` only the last command will be executed. Unless you know what you are doing, you should avoid to mix these both approaches.
Closing the robot
-----------------
To make sure that everything gets cleaned correctly after you are done using your :class:`~pypot.robot.robot.Robot`, you should always call the :meth:`~pypot.robot.robot.Robot.close` method. Doing so will ensure that all the controllers attached to this robot, and their associated dynamixel serial connection, are correctly stopped and cleaned.
.. note:: Note calling the :meth:`~pypot.robot.robot.Robot.close` method on a :class:`~pypot.robot.robot.Robot` can prevent you from opening it again without terminating your current Python session. Indeed, as the destruction of object is handled by the garbage collector, there is no mechanism which guarantee that we can automatically clean it when destroyed.
When closing the robot, we also send a stop signal to all the primitives running and wait for them to terminate. See section :ref:`my_prim` for details on what we call primitives.
.. warning:: You should be careful that all your primitives correctly respond to the stop signal. Indeed, having a blocking primitive will prevent the :meth:`~pypot.robot.robot.Robot.close` method to terminate (please refer to :ref:`start_prim` for details).
Thanks to the :func:`contextlib.closing` decorator you can easily make sure that the close function of your robot is always called whatever happened inside your code::
from contextlib import closing
import pypot.robot
# The closing decorator make sure that the close function will be called
# on the object passed as argument when the with block is exited.
with closing(pypot.robot.from_json('myconfig.json')) as my_robot:
# do stuff without having to make sure not to forget to close my_robot!
pass

17
pypot-master/pypot-master/doc/controlling_robot.rst Normal file
View File

@ -0,0 +1,17 @@
Controlling robot
*****************
Pypot handles the communication with dynamixel motors from robotis. Using a USB communication device such as USB2DYNAMIXEL or USB2AX, you can open serial communication with robotis motors (MX, RX, AX and XL320) using communication protocols TTL or RS485. More specifically, it allows easy access (both reading and writing) to the different registers of any dynamixel motors. Those registers includes values such as position, speed or torque. The whole list of registers can directly be found on the `robotis website <http://support.robotis.com/en/product/dxl_main.htm>`_.
You can access the register of the motors through two different ways:
* **Low-level API:** In the first case, you can get or set a value to a motor by directly sending a request and waiting for the motor to answer. Here, you only use the low level API to communicate with the motor (refer to section :ref:`low_level` for more details).
* **Controller API:** In the second case, you define requests which will automatically be sent at a predefined frequency. The values obtained from the requests are stored in a local copy that you can freely access at any time. However, you can only access the last synchronized value. This second method encapsulates the first approach to prevent you from writing repetitive request (refer to section :ref:`controller` for further details).
While the second approach allows the writing of simpler code without detailed knowledge of how the communication with robotis motor works, the first approach may allow for more performance through fine tuning of the communication needed in particular applications. Examples of both approaches will be provided in the next sections.
.. toctree::
dynamixel.rst
controller.rst

168
pypot-master/pypot-master/doc/dynamixel.rst Normal file
View File

@ -0,0 +1,168 @@
.. _low_level:
Dynamixel Low-level IO
======================
The low-level API almost directly encapsulates the communication protocol used by dynamixel motors. This protocol can be used to access any register of these motors. The :py:class:`~pypot.dynamixel.io.io.DxlIO` class is used to handle the communication with a particular port.
.. note:: The port can only be accessed by a single DxlIO instance.
More precisely, this class can be used to:
* open/close the communication
* discover motors (ping or scan)
* access the different control (read and write)
The communication is thread-safe to avoid collision in the communication buses.
As an example, you can write::
import itertools
with DxlIO('/dev/USB0') as dxl_io:
ids = dxl_io.scan([1, 2, 3, 4, 5])
print(dxl_io.get_present_position(ids))
dxl_io.set_goal_position(dict(zip(ids, itertools.repeat(0))))
.. note:: Since pypot version 2.2, support for the robotis protocol v2 and for XL-320 motors has been added. To avoid confusion there is a another class that should be used: :py:class:`~pypot.dynamixel.io.io_320.Dxl320IO` in this case.
.. _open_connection:
Opening/Closing a communication port
------------------------------------
In order to open a connection with the device, you will need to know what port it is connected to. Pypot has a function named :func:`~pypot.dynamixel.get_available_ports` which will try to auto-discover any compatible devices connected to the communication ports.
To create a connection, open up a python terminal and type the following code::
import pypot.dynamixel
ports = pypot.dynamixel.get_available_ports()
if not ports:
raise IOError('no port found!')
print('ports found', ports)
print('connecting on the first available port:', ports[0])
dxl_io = pypot.dynamixel.DxlIO(ports[0])
This should open a connection through a virtual communication port to your device.
.. warning:: It is important to note that it will open a connection using a default baud rate. By default your motors are set up to work on the robotis default baud rate (57140) while pypot is set up to work with a 1000000 baud rate. To communicate with your motors, you must ensure that this baud rate is the same baud rate that the motors are configure to use. So, you will need to change either the configuration of your motors (see `the Herborist tool <http://github.com/poppy-project/herborist/>`_.) or change the default baud rate of your connection.
To set up a connection with another baud rate you can write::
dxl_io = pypot.dynamixel.DxlIO(port, baudrate=57600)
The communication can be closed using the :meth:`~pypot.dynamixel.io.DxlIO.close` method.
.. note:: The class :class:`~pypot.dynamixel.io.DxlIO` can also be used as a `Context Manager <https://docs.python.org/2/library/contextlib.html>`_ (the :meth:`~pypot.dynamixel.io.DxlIO.close` method will automatically be called at the end).
For instance::
with pypot.dynamixel.DxlIO('/dev/ttyUSB0') as dxl_io:
...
Finding motors
--------------
Pypot has been designed to work specifically with the Robotis range of motors. These motors use two different protocols to communicate: TTL (3 wire bus) and RS485 (4 wire Bus). The motors can be daisy chained together with other types of motors on the same bus *as long as the bus communicates using the same protocol*. This means that MX-28 and AX-12 can communicate on the same bus, but cannot be connected to a RX-28.
All motors work sufficiently well with a 12V supply. Some motors can use more than 12V but you must be careful not to connect an 18V supply on a bus that contains motors that can only use 12V! Connect this 12V SMPS supply (switch mode power supply) to a Robotis SMPS2Dynamixel device which regulates the voltage coming from the SMPS. Connect your controller device and a single motor to this SMPS2Dynamixel.
Open your python terminal and create your :class:`~pypot.dynamixel.io.DxlIO` as described in the above section :ref:`open_connection`.
To detect the motors and find their id you can scan the bus. To avoid spending a long time searching all possible values, you can add a list of values to test::
dxl_io.scan()
>>> [4, 23, 24, 25]
dxl_io.scan([0, 1, 2, 3, 4, 5, 6, 7, 8, 9])
>>> [4]
Or, you can use the shorthand::
dxl_io.scan(range(10))
>>> [4]
This should produce a list of the ids of the motors that are connected to the bus. Each motor on the bus must have a unique id. This means that unless your motors have been configured in advance, it is better to connect them one by one to ensure they all have unique ids first.
.. note:: You also can modify the timeout to speed up the scanning. Be careful though, as this could result in loosing messages.
Low-level control
-----------------
Now we have the id of the motors connected, we can begin to access their functions by using their id. Try to find out the present position (in degrees) of the motor by typing the following::
dxl_io.get_present_position((4, ))
>>> (67.8, )
You can also write a goal position (in degrees) to the motor using the following::
dxl_io.set_goal_position({4: 0})
The motors are handled in degrees where 0 is considered the central point of the motor turn. For the MX motors, the end points are -180° and 180°. For the AX and RX motors, these end points are -150° to 150°.
.. warning:: As you can see on the example above, you should always pass the id parameter as a list. This is intended as getting a value from several motors takes the same time as getting a value from a single motor (thanks to the SYNC_READ instruction). Similarly, we use dictionary with pairs of (id, value) to set value to a specific register of motors and benefit from the SYNC_WRITE instruction.
As an example of what you can do with the low-level API, we are going to apply a sinusoid on two motors (make sure that the motion will not damage your robot before running the example!). Here is a complete listing of the code needed::
import itertools
import numpy
import time
import pypot.dynamixel
AMP = 30
FREQ = 0.5
if __name__ == '__main__':
ports = pypot.dynamixel.get_available_ports()
print('available ports:', ports)
if not ports:
raise IOError('No port available.')
port = ports[0]
print('Using the first on the list', port)
dxl_io = pypot.dynamixel.DxlIO(port)
print('Connected!')
found_ids = dxl_io.scan()
print('Found ids:', found_ids)
if len(found_ids) < 2:
raise IOError('You should connect at least two motors on the bus for this test.')
ids = found_ids[:2]
dxl_io.enable_torque(ids)
speed = dict(zip(ids, itertools.repeat(200)))
dxl_io.set_moving_speed(speed)
pos = dict(zip(ids, itertools.repeat(0)))
dxl_io.set_goal_position(pos)
t0 = time.time()
while True:
t = time.time()
if (t - t0) > 5:
break
pos = AMP * numpy.sin(2 * numpy.pi * FREQ * t)
dxl_io.set_goal_position(dict(zip(ids, itertools.repeat(pos))))
time.sleep(0.02)
Thanks to pypot, you can access all registers of your motors using the same syntax (e.g. :meth:`~pypot.dynamixel.io.DxlIO.get_present_speed`, :meth:`~pypot.dynamixel.io.DxlIO.set_max_torque`, :meth:`~pypot.dynamixel.io.DxlIO.get_pid_gain`). Some shortcuts have been provided to make the code more readable (e.g. :meth:`~pypot.dynamixel.io.DxlIO.enable_torque` instead of set_torque_enabled). All the getter functions takes a list of ids as argument and the setter takes a dictionary of (id: value) pairs. You can refer to the documentation of :class:`~pypot.dynamixel.io.DxlIO` for a complete list of all the available methods.
.. note:: Pypot provides an easy way to extend the code and automatically create methods to access new registers added by robotis.

81
pypot-master/pypot-master/doc/extending.rst Normal file
View File

@ -0,0 +1,81 @@
.. _extending:
Extending pypot
===============
While pypot has been originally designed for controlling dynamixel based robots, it became rapidly obvious that it would be really useful to easily:
* control other types of motor (e.g. servo-motors controlled using PWM)
* control an entire robot composed of different types of motors (using dynamixel for the legs and smaller servo for the hands for instance)
While it was already possible to do such things in pypot, the library has been partially re-architectured in version 2.x to better reflect those possibilities and most importantly make it easier for contributors to add the layer needed for adding support for other types of motors.
.. note:: While in most of this documentation, we will show how support for other motors can be added, similar methods can be applied to also support other sensors.
The rest of this section will describe the main concept behing pypot's architecture and then give examples of how to extend it.
Pypot's architecture
--------------------
Pypot's architecture is built upon the following basic concepts:
* **I/O**: low-level layer handling the communication with motors or sensors. This abstract layer has been designed to be as generic as possible. The idea is to keep each specific communication protocol separated from the rest of the architecture and allow for an easy replacement of an IO by another one - such an example is detailed in the next section when `dynamixel IO <http://poppy-project.github.io/pypot/pypot.dynamixel.html#module-pypot.dynamixel.io>`_ is replaced by the `communication layout with the VREP <http://poppy-project.github.io/pypot/pypot.vrep.html#module-pypot.vrep.io>`_ simulator.
* **Controller**: set of update loops to keep an (or multiple) "hardware" device(s) up to date with their "software" equivalent. This synchronization can goes only from the hard to the soft (e.g. in the case of a sensor) or both ways (e.g. for reading motor values and sending motor commands). The calls can be asynchronous or synchronous, each controller can have its own refresh frequency. An example of :class:`~pypot.robot.controller.Controller` is the :class:`~pypot.dynamixel.controller.DxlController` which synchronizes position/speed/load of all motors on a dynamixel bus in both directions.
* **Robot**: The robot layer is a pure abstraction which aims at bringing together different types of motors and sensors. This high-level is most likely to be the one accessed by the end-user which wants to directly control the motors of its robot no matter what is the IO used underneath. The robot can be directly created using a `configuration file <http://poppy-project.github.io/pypot/controller.html#writing-the-configuration>`_ describing all IO and Controllers used.
* **Primitive**: independent behaviors applied to a robot. They are not directly accessing the robot registers but are first combined through a `Primitive Manager <http://poppy-project.github.io/pypot/primitive.html>`_ which sends the results of this combination to the robot. This abstraction is used to designed behavioral-unit that can be combined into more complex behaviors (e.g. a walking primitive and and balance primitive combined to obtain a balanced-walking). Primitives are also a convenient way to monitor or remotely access a robot - ensuring some sort of sandboxing.
Those main aspects of pypot's architecture are summarized in the figure below.
.. image:: pypot-archi.jpg
:width: 60%
:align: center
Adding another layer
--------------------
If you want to add support for the brand new servo-motors in pypot or the new mindblowing sensor, you are in the right section. As an example of how you should proceed, we will describe how support for the `V-REP simulator <http://www.coppeliarobotics.com>`_ was added and how it allows for a seamless switch from real to simulated robot.
Adding support for the V-REP simulator in pypot could be sum up in three main steps:
* Writing the low-level IO for V-REP.
* Writing the controller to synchronize pypot's :class:`~pypot.robot.robot.Robot` with the V-REP's one.
* Integrates it to a :class:`~pypot.robot.robot.Robot`
Writing a new IO
++++++++++++++++
In pypot's architecture, the IO aims at providing convenient methods to access (read/write) value from a device - which could be a motor, a camera, or a simulator. It is the role of the IO to handle the communication:
* open/close the communication channel,
* encapsulate the protocol.
For example, the :class:`~pypot.dynamixel.io.DxlIO` (for dynamixel buses) open/closes the serial port and provides high-level methods for sending dynamixel packet - e.g. for getting a motor position. Similarly, writing the :class:`~pypot.vrep.io.VrepIO` consists in opening the communication socket to the V-REP simulator (thanks to `V-REP's remote API <http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm>`_) and then encapsulating all methods for getting/setting all the simulated motors registers.
.. warning:: While this is not by any mean mandatory, it is often a good practice to write all IO access as synchronous calls. The higher-level synchronization loop is usually written as a :class:`~pypot.robot.controller.AbstractController`.
The IO should also handle the low-level communication errors. For instance, the :class:`~pypot.dynamixel.io.DxlIO` automatically handles the timeout error to prevent the whole communication to stop.
.. note:: Once the new IO is written most of the integration into pypot should be done! To facilitate the integration of the new IO with the higher layer, we strongly recommend to take inspiration from the existing IO - especially the :class:`~pypot.dynamixel.io.DxlIO` and the :class:`~pypot.vrep.io.VrepIO` ones.
Writing a new Controller
++++++++++++++++++++++++
A :class:`~pypot.robot.controller.Controller` is basically a synchronization loop which role is to keep up to date the state of the device and its "software" equivalent - through the associated IO.
In the case of the :class:`~pypot.dynamixel.controller.DxlController`, it runs a 50Hz loop which reads the actual position/speed/load of the real motor and sets it to the associated register in the :class:`~pypot.dynamixel.motor.DxlMotor`. It also reads the goal position/speed/load set in the :class:`~pypot.dynamixel.motor.DxlMotor` and sends them to the "real" motor.
As most controller will have the same general structure - i.e. calling a sync. method at a predefined frequency - pypot provides an abstract class, the :class:`~pypot.robot.controller.AbstractController`, which does exactly that. If your controller fits within this conception, you should only have to overide the :meth:`~pypot.robot.controller.AbstractController.update` method.
In the case of the :class:`~pypot.vrep.controller.VrepController`, the update loop simply retrieves each motor's present position and send the new target position. A similar approach is used to retrieve values form V-REP sensors.
.. note:: Each controller can run at its own pre-defined frequency and live within its own thread. Thus, the update never blocks the main thread and you can used tight synchronization loop where they are needed (e.g. for motor's command) and slower one when latency is not a big issue (e.g. a temperature sensor).
Integrates it into the Robot
++++++++++++++++++++++++++++
Once you have defined your Controller, you most likely want to define a convenient factory functions (such as :func:`~pypot.robot.config.from_config` or :func:`~pypot.vrep.from_vrep`) allowing users to easily instantiate their :class:`~pypot.robot.robot.Robot` with the new Controller.
By doing so you will permit them to seamlessly uses your interface with this new device without changing the high-level API. For instance, as both the :class:`~pypot.dynamixel.controller.DxlController` and the :class:`~pypot.vrep.controller.VrepController` only interact with the :class:`~pypot.robot.robot.Robot` through getting and setting values into :class:`~pypot.robot.motor.Motor` instances, they can be directly switch.

61
pypot-master/pypot-master/doc/index.rst Normal file
View File

@ -0,0 +1,61 @@
.. Pypot documentation master file, created by
sphinx-quickstart on Sat Sep 15 18:52:00 2012.
You can adapt this file completely to your liking, but it should at least
contain the root `toctree` directive.
Pypot
*****
Welcome to pypot's documentation!
=================================
**Introduction**
.. toctree::
:titlesonly:
:maxdepth: 2
about.rst
installation.rst
quickstart.rst
.. _tutorial:
**Tutorial**
.. toctree::
:titlesonly:
:maxdepth: 2
controlling_robot.rst
primitive.rst
vrep.rst
extending.rst
remote_access.rst
logging.rst
**Tools**
.. toctree::
:titlesonly:
:maxdepth: 2
move.rst
**Misc**
.. toctree::
:titlesonly:
:maxdepth: 1
api.rst
FAQ.rst
Indices and tables
==================
* :ref:`genindex`
* :ref:`modindex`
* :ref:`search`

105
pypot-master/pypot-master/doc/installation.rst Normal file
View File

@ -0,0 +1,105 @@
.. _installation:
Installation
============
Requirements
-------------------
Pypot is written in `python <https://www.python.org/>`__ and need a python interpreter to be run. Moreover pypot has `scipy <www.scipy.org/>`_ and `numpy <http://www.numpy.org>`_ for dependencies, as they are not fully written in python they need system side packages to be build, it easier to use pre-build binaries for your operating system.
Windows
~~~~~~~~~~~~~~~~~~~
Install Python for Windows, then you can install pypot with `pip <#via-python-packages>`_ in the command prompt.
GNU/Linux
~~~~~~~~~~~~~~~~~~~
Use the binaries provided by your default package manager.
On Ubuntu & Debian::
sudo apt-get install python-pip python-numpy python-scipy python-matplotlib
On Fedora::
sudo yum install python-pip numpy scipy python-matplotlib
On Arch Linux::
sudo pacman -S python2-pip python2-scipy python2-numpy python2-matplotlib
After that, you can install pypot with `pip <#via-python-packages>`_.
Mac OSX
~~~~~~~~~~~~~~~~~~~
Mac OSX (unlike GNU/Linux distributions) don’t come with a package manager, but there are a couple of popular package managers you can install, like `Homebrew <http://brew.sh/>`_.
The easier way is to install `Homebrew <http://brew.sh/>`_. You have to type these commands in a terminal::
ruby -e "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/master/install)"
An use Homebrew to install python::
brew install python
After that, you can install pypot with `pip <#via-python-packages>`_.
Via Python Packages
-------------------
The pypot package is entirely written in Python. So, the install process should be rather straightforward. You can directly install it via easy_install or pip::
pip install pypot
**or**::
easy_install pypot
The up to date archive can also be directly downloaded `here <https://pypi.python.org/pypi/pypot/>`_.
If you are on a GNU/Linux operating system, you will need to execute the above commands with **sudo**.
From the source code
--------------------
You can also install it from the source. You can clone/fork our repo directly on `github <https://github.com/poppy-project/pypot>`_.
Before you start building pypot, you need to make sure that the following packages are already installed on your computer:
* `python <http://www.python.org>`_
* `pyserial <http://pyserial.sourceforge.net/>`_
* `numpy <http://www.numpy.org>`_
* `scipy <www.scipy.org/>`_
* `enum34 <https://pypi.python.org/pypi/enum34>`_
Other optional packages may be installed depending on your needs:
* `sphinx <http://sphinx-doc.org/index.html>`_ and `sphinx-bootstrap-theme <http://ryan-roemer.github.io/sphinx-bootstrap-theme/>`_ (to build the doc)
* `PyQt4 <http://www.riverbankcomputing.com/software/pyqt/intro>`_ (for the graphical tools)
* `bottle <http://bottlepy.org/>`_ and `tornado <http://www.tornadoweb.org>`_ for REST API support and http-server
Once it is done, you can build and install pypot with the classical::
cd pypot
sudo python setup.py install
Testing your install
--------------------
You can test if the installation went well with::
python -c "import pypot"
You will also have to install the driver for the USB2serial port. There are two devices that have been tested with pypot that could be used:
* USB2AX - this device is designed to manage TTL communication only
* USB2Dynamixel - this device can manage both TTL and RS485 communication.
On Windows and Mac, it will be necessary to download and install a FTDI (VCP) driver to run the USB2Dynamixel, you can find it `here <http://www.ftdichip.com/Drivers/VCP.htm>`__. Linux distributions should already come with an appropriate driver. The USB2AX device should not require a driver installation under MAC or Linux, it should already exist. For Windows XP, it should automatically install the correct driver.
.. note:: On the side of the USB2Dynamixel there is a switch. This is used to select the bus you wish to communicate on. This means that you cannot control two different bus protocols at the same time.
On most Linux distributions you will not have the necessary permission to access the serial port. You can either run the command in sudo or better you can add yourself to the *dialout* or the *uucp* group (depending on your distribution)::
sudo addgroup $USER dialout
sudo addgroup $USER uucp
At this point you should have a pypot ready to be used! In the extremely unlikely case where anything went wrong during the installation, please refer to the `issue tracker <https://github.com/poppy-project/pypot/issues>`_.

103
pypot-master/pypot-master/doc/logging.rst Normal file
View File

@ -0,0 +1,103 @@
Mastering the logging system
============================
Pypot used the Python's builtin `logging <http://docs.python.org/2/library/logging.html>`_ module for logging. For details on how to use this module please refer to Python's own documentation or the one on `django webstite <https://docs.djangoproject.com/en/1.6/topics/logging/>`_. Here, we will only describe what pypot is logging and how it is organised (see section :ref:`log_struct`). We will also present a few examples on how to use pypot logging and parse the information (see section :ref:`log_ex`).
.. _log_struct:
Logging structure
-----------------
Pypot is logging information at all different levels:
* low-level dynamixel IO
* motor and robot abstraction
* within each primitive
* each request received by the server
.. note:: As you probably do not want to log everything (pypot is sending a lot of messages!!!), you have to select in the logging structure what is relevant in your program and define it in your logging configuration.
Pypot's logging naming convention is following pypot's architecture. Here is the detail of what pypot is logging with the associated logger's name:
* The logger's name **pypot.dynamixel.io** is logging information related to opening/closing port (INFO) and each sent/received package (DEBUG). The communication and timeout error are also logged (WARNING). This logger always provides you the port name, the baudrate and timeout of your connection as extra information.
* The logger **pypot.dynamixel.motor** is logging each time a register of a motor is set (DEBUG). The name of the register, the name of the motor and the set value are given in the message.
* **pypot.robot.config** is logging information regarding the creation of a robot through a config dictionary. A message is sent for each motor, controller and alias added (INFO). A WARNING message is also sent when the angle limits of a motor are changed. We provide as extra the entire config dictionary.
* The logger **pypot.robot.robot** is logging when the synchronization is started/stopped (INFO) and when a primitive is attached (INFO).
* **pypot.primitive.primitive** logs a message when the primitive is started/stopped and paused/resumed (INFO). Eeach :meth:`~pypot.primitive.primitive.LoopPrimitive.update` of a LoopPrimitive is also logged (DEBUG). Each time a primitive sets a value to a register a message is also logged (DEBUG).
* **pypot.primitive.manager** provides you information on how the values sent within primitives were combined (DEBUG).
* **pypot.server** logs when the server is started (INFO) and each handled request (DEBUG).
.. _log_ex:
Using Pypot's logging
---------------------
As an example of what you can do with the logging system, here is how you can check the "real" update frequency of a loop primitive.
First, you have to define a logging config. As you can see, here we specify that we only want the log coming form 'pypot.primitive' and the message is formatted so we only keep the timestamp::
LOGGING = {
'version': 1,
'disable_existing_loggers': True,
'formatters': {
'time': {
'format': '%(asctime)s',
},
},
'handlers': {
'file': {
'level': 'DEBUG',
'class': 'logging.FileHandler',
'filename': 'pypot.log',
'formatter': 'time',
},
},
'loggers': {
'pypot.primitive': {
'handlers': ['file', ],
'level': 'DEBUG',
},
},
}
Then, we just have to write a simple program, where for instance we run our dummy primitive for ten seconds::
import pypot.robot
[...]
if __name__ == '__main__':
logging.config.dictConfig(LOGGING)
r = pypot.robot.from_config(ergo_config)
class DummyPrimitive(LoopPrimitive):
pass
p = DummyPrimitive(r, 50)
p.start()
time.sleep(10)
p.stop()
The execution of the program above will create a file named 'pypot.log' where each line corresponds to the timestamp of each primitive update. This file can then be easily parsed::
t = []
with open('pypot.log') as f:
for l in f.readlines():
d = datetime.datetime.strptime('%Y-%m-%d %H:%M:%S,%f\n')
t.append(d)
t = numpy.array(t)
dt = map(lambda dt: dt.total_seconds(), numpy.diff(t))
dt = numpy.array(dt) * 1000
print(numpy.mean(dt), numpy.std(dt))
plot(dt)
show()

48
pypot-master/pypot-master/doc/move.rst Normal file
View File

@ -0,0 +1,48 @@
.. _move:
Move recording and playing
==========================
The :mod:`~pypot.primitive.move` module contains utility classes to help you record and play moves. Those :class:`~pypot.primitive.move.Move` is simply defined as a sequence of positions.
.. note:: To keep the :mod:`~pypot.primitive.move` module as simple as possible, you can only define :class:`~pypot.primitive.move.Move` as a predefined frequency and you can not define keyframes whenever you want. This could be added if it seems like it would be useful.
You can use the :mod:`~pypot.primitive.move` module to:
* record moves,
* play moves,
* save/load them on the disk.
The :class:`~pypot.primitive.move.MoveRecorder` and :class:`~pypot.primitive.move.MovePlayer` are defined as subclass of :class:`~pypot.primitive.primitive.LoopPrimitive` and can thus be used as such. For instance, if you want to record a 50Hz move on all the motor of an ergo-robot you can simply use the following code::
import time
import pypot.robot
from pypot.primitive.move import MoveRecorder, Move, MovePlayer
ergo = pypot.robot.from_config(...)
move_recorder = MoveRecorder(ergo, 50, ergo.motors)
ergo.compliant = True
move_recorder.start()
time.sleep(5)
move_recorder.stop()
This move can then be saved on disk::
with open('my_nice_move.move', 'w') as f:
move_recorder.move.save(f)
And loaded and replayed::
with open('my_nice_move.move') as f:
m = Move.load(f)
ergo.compliant = False
move_player = MovePlayer(ergo, m)
move_player.start()
.. warning:: It is important to note that you should be sure that you primitive actually runs at the same speed that the move has been recorded. If the player can not run as fast as the framerate of the recorded :class:`~pypot.primitive.move.Move`, it will be played slowly resulting in a slower version of your move.

BIN
pypot-master/pypot-master/doc/poppy-creatures.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 299 KiB

147
pypot-master/pypot-master/doc/primitive.rst Normal file
View File

@ -0,0 +1,147 @@
.. _my_prim:
Primitives everywhere
=====================
In the previous sections, we have shown how to make a simple behavior thanks to the :class:`~pypot.robot.robot.Robot` abstraction. But how to combine those elementary behaviors into something more complex? You could use threads and do it manually, but we provide the :class:`~pypot.primitive.primitive.Primitive` to abstract most of the work for you.
What do we call "Primitive"?
----------------------------
We call :class:`~pypot.primitive.primitive.Primitive` any simple or complex behavior applied to a :class:`~pypot.robot.robot.Robot`. A primitive can access all sensors and effectors in the robot. A primitive is supposed to be independent of other primitives. In particular, a primitive is not aware of the other primitives running on the robot at the same time. We imagine those primitives as elementary blocks that can be combined to create more complex blocks in a hierarchical manner.
.. note:: The independence of primitives is really important when you create complex behaviors - such as balance - where many primitives are needed. Adding another primitive - such as walking - should be direct and not force you to rewrite everything. Furthermore, the balance primitive could also be combined with another behavior - such as shoot a ball - without modifying it.
To ensure this independence, the primitive is running in a sort of sandbox. More precisely, this means that the primitive has not direct access to the robot. It can only request commands (e.g. set a new goal position of a motor) to a :class:`~pypot.primitive.manager.PrimitiveManager` which transmits them to the "real" robot. As multiple primitives can run on the robot at the same time, their request orders are combined by the manager.
.. note:: The primitives all share the same manager. In further versions, we would like to move from this linear combination of all primitives to a hierarchical structure and have different layer of managers.
The manager uses a filter function to combine all orders sent by primitives. By default, this filter function is a simple mean but you can choose your own specific filter (e.g. add function).
.. warning:: You should not mix control through primitives and direct control through the :class:`~pypot.robot.robot.Robot`. Indeed, the primitive manager will overwrite your orders at its refresh frequency: i.e. it will look like only the commands send through primitives will be taken into account.
.. _write_own_prim:
Writing your own primitive
--------------------------
To write you own primitive, you have to subclass the :class:`~pypot.primitive.primitive.Primitive` class. It provides you with basic mechanisms (e.g. connection to the manager, setup of the thread) to allow you to directly "plug" your primitive to your robot and run it.
.. note:: You should always call the super constructor if you override the :meth:`~pypot.primitive.primitive.Primitive.__init__` method.
As an example, let's write a simple primitive that recreate the dance behavior written in the :ref:`dance_` section. Notice that to pass arguments to your primitive, you have to override the :meth:`~pypot.primitive.primitive.Primitive.__init__` method::
import time
import pypot.primitive
class DancePrimitive(pypot.primitive.Primitive):
def __init__(self, robot, amp=30, freq=0.5):
self.robot = robot
self.amp = amp
self.freq = freq
pypot.primitive.Primitive.__init__(self, robot)
def run(self):
amp = self.amp
freq = self.freq
# self.elapsed_time gives you the time (in s) since the primitive has been running
while self.elapsed_time < 30:
x = amp * numpy.sin(2 * numpy.pi * freq * self.elapsed_time)
self.robot.base_pan.goal_position = x
self.robot.head_pan.goal_position = -x
time.sleep(0.02)
To run this primitive on your robot, you simply have to do::
ergo_robot = pypot.robot.from_config(...)
dance = DancePrimitive(ergo_robot,amp=60, freq=0.6)
dance.start()
If you want to make the dance primitive infinite you can use the :class:`~pypot.primitive.primitive.LoopPrimitive` class::
class LoopDancePrimitive(pypot.primitive.LoopPrimitive):
def __init__(self, robot, refresh_freq, amp=30, freq=0.5):
self.robot = robot
self.amp = amp
self.freq = freq
LoopPrimitive.__init__(self, robot, refresh_freq)
# The update function is automatically called at the frequency given on the constructor
def update(self):
amp = self.amp
freq = self.freq
x = amp * numpy.sin(2 * numpy.pi * freq * self.elapsed_time)
self.robot.base_pan.goal_position = x
self.robot.head_pan.goal_position = -x
And then runs it with::
ergo_robot = pypot.robot.from_config(...)
dance = LoopDancePrimitive(ergo_robot, 50, amp = 40, freq = 0.3)
# The robot will dance until you call dance.stop()
dance.start()
.. warning:: When writing your own primitive, you should always keep in mind that you should never directly pass the robot or its motors as argument and access them directly. You have to access them through the self.robot and self.robot.motors properties. Indeed, at instantiation the :class:`~pypot.robot.robot.Robot` (resp. :class:`~pypot.dynamixel.motor.DxlMotor`) instance is transformed into a :class:`~pypot.primitive.primitive.MockupRobot` (resp. :class:`~pypot.primitive.primitive.MockupMotor`). Those class are used to intercept the orders sent and forward them to the :class:`~pypot.primitive.manager.PrimitiveManager` which will combine them. By directly accessing the "real" motor or robot you circumvent this mechanism and break the sandboxing. If you have to specify a list of motors to your primitive (e.g. apply the sinusoid primitive to the specified motors), you should either give the motors name and access the motors within the primitive or transform the list of :class:`~pypot.dynamixel.motor.DxlMotor` into :class:`~pypot.primitive.primitive.MockupMotor` thanks to the :meth:`~pypot.primitive.primitive.Primitive.get_mockup_motor` method.
For instance::
class MyDummyPrimitive(pypot.primitive.Primitive):
def run(self, motors_name):
motors = [getattr(self.robot, name) for name in motors_name]
while True:
for m in fake_motors:
...
or::
class MyDummyPrimitive(pypot.primitive.Primitive):
def run(self, motors):
fake_motors = [self.get_mockup_motor(m) for m in motors]
while True:
for m in fake_motors:
...
.. _start_prim:
Start, Stop, Pause, and Resume
------------------------------
The primitive can be :meth:`~pypot.primitive.primitive.Primitive.start`, :meth:`~pypot.primitive.primitive.Primitive.stop`, :meth:`~pypot.utils.stoppablethread.StoppableThread.pause` and :meth:`~pypot.utils.stoppablethread.StoppableThread.resume`. Unlike regular python thread, primitive can be restart by calling again the :meth:`~pypot.primitive.primitive.Primitive.start` method.
When overriding the :class:`~pypot.primitive.primitive.Primitive`, you are responsible for correctly handling those events. For instance, the stop method will only trigger the should stop event that you should watch in your run loop and break it when the event is set. In particular, you should check the :meth:`~pypot.utils.stoppablethread.StoppableThread.should_stop` and :meth:`~pypot.utils.stoppablethread.StoppableThread.should_pause` in your run loop. You can also use the :meth:`~pypot.utils.stoppablethread.StoppableThread.wait_to_stop` and :meth:`~pypot.utils.stoppablethread.StoppableThread.wait_to_resume` to wait until the commands have really been executed.
.. note:: You can refer to the source code of the :class:`~pypot.primitive.primitive.LoopPrimitive` for an example of how to correctly handle all these events.
Attaching a primitive to the robot
----------------------------------
In the previous section, we explain that the primitives run in a sandbox in the sense that they are not aware of the other primitives running at the same time. In fact, this is not exactly true. More precisely, a primitive can access everything attached to the robot: e.g. motors, sensors. But you can also attach a primitive to the robot.
Let's go back on our DancePrimitive example. You can write::
ergo_robot = pypot.robot.from_config(...)
ergo_robot.attach_primitive(DancePrimitive(ergo_robot), 'dance')
ergo_robot.dance.start()
By attaching a primitive to the robot, you make it accessible from within other primitive.
For instance you could then write::
class SelectorPrimitive(pypot.primitive.Primitive):
def run(self):
if song == 'my_favorite_song_to_dance' and not self.robot.dance.is_alive():
self.robot.dance.start()
.. note:: In this case, instantiating the DancePrimitive within the SelectorPrimitive would be another solution.

BIN
pypot-master/pypot-master/doc/pypot-archi.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 154 KiB

81
pypot-master/pypot-master/doc/pypot.dynamixel.rst Normal file
View File

@ -0,0 +1,81 @@
:mod:`dynamixel` Package
========================
.. autofunction:: pypot.dynamixel.get_available_ports
.. autofunction:: pypot.dynamixel.find_port
.. autofunction:: pypot.dynamixel.autodetect_robot
:mod:`~pypot.dynamixel.io` Package
----------------------------------
.. automodule:: pypot.dynamixel.io
:members:
:undoc-members:
:show-inheritance:
.. automodule:: pypot.dynamixel.io.io
:members:
:undoc-members:
:show-inheritance:
.. automodule:: pypot.dynamixel.io.io_320
:members:
:undoc-members:
:show-inheritance:
:mod:`motor` Module
-------------------
.. automodule:: pypot.dynamixel.motor
:members:
:show-inheritance:
.. autoattribute:: pypot.dynamixel.motor.DxlMotor.registers
:mod:`controller` Module
------------------------
.. automodule:: pypot.dynamixel.controller
:members:
:undoc-members:
:show-inheritance:
:mod:`error` Module
-------------------
.. automodule:: pypot.dynamixel.error
:members:
:undoc-members:
:show-inheritance:
:mod:`conversion` Module
------------------------
.. automodule:: pypot.dynamixel.conversion
:members:
:undoc-members:
:show-inheritance:
:mod:`protocol` Package
-----------------------
:mod:`v1` Module
++++++++++++++++
.. automodule:: pypot.dynamixel.protocol.v1
:members:
:undoc-members:
:show-inheritance:
:mod:`v2` Module
++++++++++++++++
.. automodule:: pypot.dynamixel.protocol.v2
:members:
:undoc-members:
:show-inheritance:

36
pypot-master/pypot-master/doc/pypot.primitive.rst Normal file
View File

@ -0,0 +1,36 @@
:mod:`primitive` Package
========================
:mod:`~pypot.primitive.primitive` Module
----------------------------------------
.. automodule:: pypot.primitive.primitive
:members:
:undoc-members:
:show-inheritance:
:mod:`~pypot.primitive.manager` Module
--------------------------------------
.. automodule:: pypot.primitive.manager
:members:
:undoc-members:
:show-inheritance:
:mod:`~pypot.primitive.move` Module
------------------------------------
.. automodule:: pypot.primitive.move
:members:
:undoc-members:
:show-inheritance:
:mod:`~pypot.primitive.utils` Module
------------------------------------
.. automodule:: pypot.primitive.utils
:members:
:undoc-members:
:show-inheritance:

64
pypot-master/pypot-master/doc/pypot.robot.rst Normal file
View File

@ -0,0 +1,64 @@
:mod:`robot` Package
====================
.. automodule:: pypot.robot
:members:
:undoc-members:
:show-inheritance:
:mod:`robot` Module
-------------------
.. automodule:: pypot.robot.robot
:members:
:undoc-members:
:show-inheritance:
:mod:`motor` Module
-------------------
.. automodule:: pypot.robot.motor
:members:
:undoc-members:
:show-inheritance:
:mod:`sensor` Module
--------------------
.. automodule:: pypot.robot.sensor
:members:
:undoc-members:
:show-inheritance:
:mod:`controller` Module
------------------------
.. automodule:: pypot.robot.controller
:members:
:undoc-members:
:show-inheritance:
:mod:`io` Module
----------------
.. automodule:: pypot.robot.io
:members:
:undoc-members:
:show-inheritance:
:mod:`config` Module
--------------------
.. automodule:: pypot.robot.config
:members:
:undoc-members:
:show-inheritance:
:mod:`remote` Module
--------------------
.. automodule:: pypot.robot.remote
:members:
:undoc-members:
:show-inheritance:

24
pypot-master/pypot-master/doc/pypot.sensor.rst Normal file
View File

@ -0,0 +1,24 @@
:mod:`sensor` Package
=====================
.. automodule:: pypot.sensor
:members:
:undoc-members:
:show-inheritance:
:mod:`kinect` Module
--------------------
.. automodule:: pypot.sensor.kinect.sensor
:members:
:undoc-members:
:show-inheritance:
:mod:`optitrack` Module
-----------------------
.. automodule:: pypot.sensor.optitrack
:members:
:undoc-members:
:show-inheritance:

40
pypot-master/pypot-master/doc/pypot.server.rst Normal file
View File

@ -0,0 +1,40 @@
:mod:`server` Package
=====================
.. automodule:: pypot.server
:members:
:undoc-members:
:show-inheritance:
:mod:`rest` Module
---------------------
.. automodule:: pypot.server.rest
:members:
:undoc-members:
:show-inheritance:
:mod:`httpserver` Module
------------------------
.. automodule:: pypot.server.httpserver
:members:
:undoc-members:
:show-inheritance:
:mod:`zmqserver` Module
-----------------------
.. automodule:: pypot.server.zmqserver
:members:
:undoc-members:
:show-inheritance:
:mod:`snap` Module
-----------------------
.. automodule:: pypot.server.snap
:members:
:undoc-members:
:show-inheritance:

17
pypot-master/pypot-master/doc/pypot.tools.rst Normal file
View File

@ -0,0 +1,17 @@
pypot.tools package
===================
Submodules
----------
.. toctree::
pypot.tools.dxl_reset module
----------------------------
.. automodule:: pypot.tools.dxl_reset
:members:
:undoc-members:
:show-inheritance:

24
pypot-master/pypot-master/doc/pypot.utils.rst Normal file
View File

@ -0,0 +1,24 @@
:mod:`utils` Package
====================
.. automodule:: pypot.utils
:members:
:undoc-members:
:show-inheritance:
:mod:`stoppablethread` Module
-----------------------------
.. automodule:: pypot.utils.stoppablethread
:members:
:undoc-members:
:show-inheritance:
:mod:`trajectory` Module
-----------------------------
.. automodule:: pypot.utils.trajectory
:members:
:undoc-members:
:show-inheritance:

25
pypot-master/pypot-master/doc/pypot.vrep.rst Normal file
View File

@ -0,0 +1,25 @@
:mod:`vrep` Package
===================
.. automodule:: pypot.vrep
:members:
:undoc-members:
:show-inheritance:
:mod:`io` Module
----------------
.. automodule:: pypot.vrep.io
:members:
:undoc-members:
:show-inheritance:
:mod:`controller` Module
------------------------
.. automodule:: pypot.vrep.controller
:members:
:undoc-members:
:show-inheritance:

BIN
pypot-master/pypot-master/doc/pypot_logo-144x144.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 6.3 KiB

BIN
pypot-master/pypot-master/doc/pypot_logo-48x48.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 2.4 KiB

BIN
pypot-master/pypot-master/doc/pypot_logo.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 10 KiB

132
pypot-master/pypot-master/doc/quickstart.rst Normal file
View File

@ -0,0 +1,132 @@
.. _quickstart:
QuickStart: playing with an Ergo-Robot
======================================
To let you discover what you can do with pypot, in this section we describe the few steps required to make a robot build from robotis motor dance. This short introduction will in particular describe you:
* how to define your robot within the software,
* how to connect it to your computer,
* and finally how to control it.
We have developed in our team the Ergo-Robot as a way to explore large scale long term robotic experiments outside of the lab and we have made the whole hardware and software architecture available publicly in an open-source manner so that other research team in the world can use it and leverage our efforts for their own research. As a consequence, you can easily build your own Ergo-Robot.
In this Quick Start, we will use this robot as a base and thus assume that you are using such a robot. Obviously, you can transpose all the following examples to any particular robot made from robotis motor.
Building your own Ergo-Robot
----------------------------
Ergo-Robots have been developed for an art exhibition in Fondation Cartier: `Mathematics a beautiful elsewhere <http://fondation.cartier.com/en/art-contemporain/26/exhibitions/294/all-the-exhibitions/89/mathematics-a-beautiful-elsewhere/>`__. They are small creatures made from robotis motors and shaped as a stem with a head designed by David Lynch. They were developed to explore research topics such as artificial curiosity and language games. The robots were used during 5 months at the exhibition. More details on the whole project can be found `here <https://www.poppy-project.org/project/mathematics-a-beautiful-elsewhere>`__.
.. image:: ErgoRobots.jpg
:height: 400
:align: center
The complete instructions to build your own Ergo-Robot are available `here <https://wiki.bordeaux.inria.fr/flowers/doku.php?id=robot:ergorobot:construction>`__.
Connecting the robot to your computer
-------------------------------------
Now that you have your own robot, let's start writing the code necessary to control it.
The first step is to setup the configuration for your robot. It will describe the motor configuration of your robot, the USB2serial controller used and make the initialization really easy. Configurations are described as Python dictionaries. Yet, they can be quite repetitive to write. Luckily, the pypot package comes with an example of a configuration for an Ergo-Robot. You can first import it, so you can modify it::
from pypot.robot.config import ergo_robot_config
If you do some introspection on this object, you will see that it is just a regular Python dictionary. So you can directly edit it like you will do with any other dict. You can also copy the one provided with pypot and work with your own copy::
my_config = dict(ergo_robot_config)
my_config['controllers']['my_dxl_controller']['port'] = 'COM6' # For Windows' users
You will only have to modify the USB2serial port and the id of the motors so they correspond to your robot. If you do not know how to get this information, you can refer to the documentation on the `Herborist tool <http://github.com/poppy-project/herborist/>`__.
Alternatively, you can directly ask pypot::
import pypot.dynamixel
print(pypot.dynamixel.get_available_ports())
['/dev/tty.usbserial-A4008aCD', '/dev/tty.usbmodemfd1311']
dxl_io = pypot.dynamixel.DxlIO('/dev/tty.usbserial-A4008aCD')
print(dxl_io.scan())
[11, 12, 13, 14, 15, 16]
.. note:: You can save/load configurations from any format that can be written/read as a Python dictionary. A wrapper for loading json configuration file is provided (see :func:`~pypot.robot.config.from_json`).
.. note:: Since pypot 1.7, you can now set the port to 'auto' in the dictionary. When loading the configuration, pypot will automatically try to find the port with the corresponding attached motor ids.
For instance::
my_config = dict(ergo_robot_config)
my_config['controllers']['my_dxl_controller']['port'] = 'auto'
Once you have edited the configuration dictionary, you should be able to instantiate your robot directly like this::
import pypot.robot
ergo_robot = pypot.robot.from_config(my_config)
At this point, if you have not seen any errors it means that you are successfully connected to your robot! You can find details on how to write more complex configuration file in the :ref:`config_file` section.
.. _dance_:
Controlling your Ergo-Robot
---------------------------
Now that you are connected to your Ergo-Robot, let's write a very simple program to make it dance a bit.
First, write the following lines to start you robot (we assume that you have correctly setup your configuration)::
import pypot.robot
ergo_robot = pypot.robot.from_config(my_config)
Everything should be clear now. When creating your robot, it automatically starts the synchronization between the "software" robot and the real one, i.e. all commands that you will send in python code will automatically be sent to the physical Ergo-Robot (for details on the underlying mechanisms, see :ref:`Sync Loop <sync_loop>`).
Now, we are going to put the robot in its initial position::
for m in ergo_robot.motors:
m.compliant = False
# Go to the position 0 within 2 seconds.
# Note that the position is expressed in degrees.
m.goto_position(0, 2)
The robot should raise and smoothly go to its base position. Now, we are going to move it to a more stable position. We will use it as a rest position for our dance::
rest_pos = {'base_tilt_lower': 45,
'base_tilt_upper': -45,
'head_tilt_lower': 30,
'head_tilt_upper': -30}
# You can directly set new positions to motors by providing
# the Robot goto_position method with a dictionary such as
# {motor_name: position, motor_name: position...}
ergo_robot.goto_position(rest_pos, duration=1, wait=True)
We will now create a very simple dance just by applying two sinus with opposite phases on the base and head motors of the robot::
import numpy
import time
amp = 30
freq = 0.5
# As you can notice, property to access the motors defined
# in the configuration file are automatically created.
ergo_robot.base_pan.moving_speed = 0 # 0 corresponds to the max speed
ergo_robot.head_pan.moving_speed = 0
t0 = time.time()
while True:
t = time.time() - t0
if t > 10:
break
x = amp * numpy.sin(2 * numpy.pi * freq * t)
ergo_robot.base_pan.goal_position = x
ergo_robot.head_pan.goal_position = -x
time.sleep(0.02)
Your robot should start dancing for ten seconds. Now, that you have seen the very basic things that you can do with pypot. It is time to jump on the :ref:`tutorial <tutorial>` to get a complete overview of the possibility.

42
pypot-master/pypot-master/doc/remote_access.rst Normal file
View File

@ -0,0 +1,42 @@
REST API
========
We add the possibility to remotely access and control your robot through TCP network. This can be useful both to work with client/server architecture (e.g. to separate the low-level control running on an embedded computer and higher-level computation on a more powerful computer) and to allow you to plug your existing code written in another language to the pypot's API.
We defined a protocol which permits the access of all the robot variables and method (including motors and primitives) via a JSON request. The protocol is entirely described in the section :ref:`remote_protocol` below. Two transport methods have been developed so far:
* HTTP via GET and POST request (see the :class:`~pypot.server.httpserver.HTTPRobotServer`)
* ZMQ socket (see the :class:`~pypot.server.zmqserver.ZMQRobotServer`)
The :class:`~pypot.server.rest.RESTRobot` has been abstracted from the server, so you can easily add new transport methods if needed.
As an example of what you can do, here is the code of getting the load of a motor and changing its position::
import zmq
import threading
robot = pypot.robot.from_config(...)
server = pypot.server.ZMQServer(robot, host, port)
# We launch the server inside a thread
threading.Thread(target=lambda: server.run()).start()
c = zmq.Context()
s = c.socket(zmq.REQ)
req = {"robot": {"get_register_value": {"motor": "m2", "register": "present_load"}}}
s.send_json(req)
answer = s.recv_json()
print(answer)
req = {"robot": {"set_register_value": {"motor": "m2", "register": "goal_position", "value": 20}}}
s.send_json(req)
answer = s.recv_json()
print(answer)
.. _remote_protocol:
Protocol
--------
The entire protocol is entirely described `here <https://github.com/poppy-project/pypot/blob/master/REST-APIs.md>`_.

47
pypot-master/pypot-master/doc/vrep.rst Normal file
View File

@ -0,0 +1,47 @@
Using a simulated robot with V-REP
**********************************
Connecting to V-REP
-------------------
As it is often easier to work in simulation rather than with the real robot, pypot has been linked with the `V-REP simulator <http://www.coppeliarobotics.com>`_. It is described as the "Swiss army knife among robot simulators" and is a very powerful tool to quickly (re)create robotics setup. As presenting V-REP is way beyond the scope of this tutorial, we will here assume that you are already familiar with this tool. Otherwise, you should directly refer to `V-REP documentation <http://www.coppeliarobotics.com/helpFiles/index.html>`_.
Details about how to connect pypot and V-REP can be found in `this post <https://forum.poppy-project.org/t/howto-connect-pypot-to-your-simulated-version-of-poppy-humanoid-in-v-rep/332>`_.
The connection between pypot and V-REP was designed to let you seamlessly switch from your real robot to the simulated one. It is based on `V-REP's remote API <http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm>`_.
In order to connect to V-REP through pypot, you will only need to install the `V-REP <http://www.coppeliarobotics.com/downloads.html>`_ simulator. Pypot comes with a specific :class:`~pypot.vrep.io.VrepIO` designed to communicate with V-REP through its `remote API <http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm>`_.
This IO can be used to:
* connect to the V-REP server : :class:`~pypot.vrep.io.VrepIO`
* load a scene : :meth:`~pypot.vrep.io.VrepIO.load_scene`
* start/stop/restart a simulation : :meth:`~pypot.vrep.io.VrepIO.start_simulation`, :meth:`~pypot.vrep.io.VrepIO.stop_simulation`, :meth:`~pypot.vrep.io.VrepIO.restart_simulation`
* pause/resume the simulation : :meth:`~pypot.vrep.io.VrepIO.pause_simulation`, :meth:`~pypot.vrep.io.VrepIO.resume_simulation`
* get/set a motor position : :meth:`~pypot.vrep.io.VrepIO.get_motor_position`, :meth:`~pypot.vrep.io.VrepIO.set_motor_position`
* get an object position/orienation : :meth:`~pypot.vrep.io.VrepIO.get_object_position`, :meth:`~pypot.vrep.io.VrepIO.get_object_orientation`
Switch between the simulation and the real robot in a single line of code
-------------------------------------------------------------------------
As stated above, the bridge between V-REP and pypot has been designed to let you easily switch from the robot to the simulated version. In most case, you should only have to change the way you instantiate your robot::
# Working with the real robot
import pypot.robot
poppy = pypot.robot.from_config(config)
poppy.walk.start()
will become::
# Working with the simulated version
import pypot.vrep
poppy = pypot.vrep.from_vrep(config, vrep_host, vrep_port, vrep_scene)
poppy.walk.start()
In particular, the walking primitive should work exactly the same way in both cases without needing to change anything.
.. note:: Not all dynamixel registers have their V-REP equivalent. For the moment, only the control of the position is used. More advanced features can be easily added thanks to the controller abstraction (see section :ref:`extending`).

5
pypot-master/pypot-master/pypot/__init__.py Normal file
View File

@ -0,0 +1,5 @@
import logging
from ._version import __version__
logging.getLogger(__name__).addHandler(logging.NullHandler())

1
pypot-master/pypot-master/pypot/_version.py Normal file
View File

@ -0,0 +1 @@
__version__ = '5.0.2'

26
pypot-master/pypot-master/pypot/creatures/__init__.py Normal file
View File

@ -0,0 +1,26 @@
import sys
from .abstractcreature import AbstractPoppyCreature
module = sys.modules[__name__]
installed_poppy_creatures = {}
# Feel free to make a pull request to add your own creature here
existing_creatures = ['poppy-humanoid', 'poppy-torso', 'poppy-ergo-jr',
'poppy-ergo-starter', 'poppy-6dof-right-arm',
'poppy-dragster-mini', 'poppy-ergo', 'roboticia-quattro',
'roboticia-first', 'roboticia-uno', 'roboticia-drive',
'doggy']
for creature in existing_creatures:
package = creature.replace('-', '_')
cls_name = ''.join(x.capitalize() or '_' for x in package.split('_'))
try:
cls = getattr(__import__(package), cls_name)
installed_poppy_creatures[creature] = cls
setattr(module, cls_name, cls)
except (ImportError, AttributeError):
pass

201
pypot-master/pypot-master/pypot/creatures/abstractcreature.py Normal file
View File

@ -0,0 +1,201 @@
import sys
import logging
import json
import os
import re
from threading import Thread
from pypot.robot import Robot, from_json, use_dummy_robot
from pypot.server.snap import SnapRobotServer, find_local_ip
logger = logging.getLogger(__name__)
MAX_SETUP_TRIALS = 10
class classproperty(property):
def __get__(self, cls, owner):
return self.fget.__get__(None, owner)()
def camelcase_to_underscore(name):
return re.sub('([a-z])([A-Z0-9])', r'\1_\2', name).lower()
class AbstractPoppyCreature(Robot):
""" Abstract Class for Any Poppy Creature. """
def __new__(cls,
base_path=None, config=None,
simulator=None, scene=None, host='localhost', port=19997, id=None, shared_vrep_io=None,
use_snap=False, snap_host='0.0.0.0', snap_port=6969, snap_quiet=True,
use_http=False, http_host='0.0.0.0', http_port=8080, http_quiet=True,
use_remote=False, remote_host='0.0.0.0', remote_port=4242,
use_ws=False, ws_host='0.0.0.0', ws_port=9009,
start_background_services=True, sync=True,
**extra):
""" Poppy Creature Factory.
Creates a Robot (real or simulated) and specifies it to make it a specific Poppy Creature.
:param str config: path to a specific json config (if None uses the default config of the poppy creature - e.g. poppy_humanoid.json)
:param str simulator: name of the simulator used : 'vrep', 'poppy-simu', or 'dummy'
:param str scene: specify a particular simulation scene (if None uses the default scene of the poppy creature, use "keep-existing" to keep the current VRep scene - e.g. poppy_humanoid.ttt)
:param str host: host of the simulator
:param int port: port of the simulator
:param int id: robot id in simulator (useful when using a scene with multiple robots)
:param vrep_io: use an already connected VrepIO (useful when using a scene with multiple robots)
:type vrep_io: :class:`~pypot.vrep.io.VrepIO`
:param bool use_snap: start or not the Snap! API
:param str snap_host: host of Snap! API
:param int snap_port: port of the Snap!
:param bool use_http: start or not the HTTP API
:param str http_host: host of HTTP API
:param int http_port: port of the HTTP API
:param int id: id of robot in the v-rep scene (not used yet!)
:param bool sync: choose if automatically starts the synchronization loops
You can also add extra keyword arguments to disable sensor. For instance, to use a DummyCamera, you can add the argument: camera='dummy'.
.. warning:: You can not specify a particular config when using a simulated robot!
"""
if config and simulator:
raise ValueError('Cannot set a specific config '
'when using a simulated version!')
creature = camelcase_to_underscore(cls.__name__)
base_path = (os.path.dirname(__import__(creature).__file__)
if base_path is None else base_path)
default_config = os.path.join(os.path.join(base_path, 'configuration'),
'{}.json'.format(creature))
if config is None:
config = default_config
if simulator is not None:
if simulator == 'vrep':
from pypot.vrep import from_vrep, VrepConnectionError
scene_path = os.path.join(base_path, 'vrep-scene')
if scene != "keep-existing":
if scene is None:
scene = '{}.ttt'.format(creature)
elif not os.path.exists(scene):
if ((os.path.basename(scene) != scene) or
(not os.path.exists(os.path.join(scene_path, scene)))):
raise ValueError('Could not find the scene "{}"!'.format(scene))
scene = os.path.join(scene_path, scene)
# TODO: use the id so we can have multiple poppy creatures
# inside a single vrep scene
# vrep.simxStart no longer listen on localhost
if host == 'localhost':
host = '127.0.0.1'
try:
poppy_creature = from_vrep(config, host, port, scene if scene != "keep-existing" else None, id=id, shared_vrep_io=shared_vrep_io)
except VrepConnectionError:
raise IOError('Connection to V-REP failed!')
elif simulator == 'poppy-simu':
use_http = True
poppy_creature = use_dummy_robot(config)
elif simulator == 'dummy':
poppy_creature = use_dummy_robot(config)
else:
raise ValueError('Unknown simulation mode: "{}"'.format(simulator))
poppy_creature.simulated = True
else:
for _ in range(MAX_SETUP_TRIALS):
try:
poppy_creature = from_json(config, sync, **extra)
logger.info('Init successful')
break
except Exception as e:
logger.warning('Init fail: {}'.format(str(e)))
exc_type, exc_inst, tb = sys.exc_info()
else:
import traceback
traceback.print_tb(tb)
raise OSError('Could not initialize robot: {}'.format(exc_inst))
poppy_creature.simulated = False
with open(config) as f:
poppy_creature.config = json.load(f)
urdf_file = os.path.join(os.path.join(base_path,
'{}.urdf'.format(creature)))
poppy_creature.urdf_file = urdf_file
if use_snap:
poppy_creature.snap = SnapRobotServer(
poppy_creature, snap_host, snap_port, quiet=snap_quiet)
snap_url = 'http://snap.berkeley.edu/snapsource/snap.html'
block_url = 'http://{}:{}/snap-blocks.xml'.format(find_local_ip(), snap_port)
url = '{}#open:{}'.format(snap_url, block_url)
logger.info('SnapRobotServer is now running on: http://{}:{}\n'.format(snap_host, snap_port))
logger.info('You can open Snap! interface with loaded blocks at "{}"\n'.format(url))
if use_http:
from pypot.server.httpserver import HTTPRobotServer
poppy_creature.http = HTTPRobotServer(poppy_creature, http_host, http_port,
cross_domain_origin="*", quiet=http_quiet)
logger.info('HTTPRobotServer is now running on: http://{}:{}\n'.format(http_host, http_port))
if use_remote:
from pypot.server import RemoteRobotServer
poppy_creature.remote = RemoteRobotServer(poppy_creature, remote_host, remote_port)
logger.info('RemoteRobotServer is now running on: http://{}:{}\n'.format(remote_host, remote_port))
if use_ws:
from pypot.server import WsRobotServer
poppy_creature.ws = WsRobotServer(poppy_creature, ws_host, ws_port)
logger.info('Ws server is now running on: ws://{}:{}\n'.format(ws_host, ws_port))
cls.setup(poppy_creature)
if start_background_services:
cls.start_background_services(poppy_creature)
return poppy_creature
@classmethod
def start_background_services(cls, robot, services=['snap', 'http', 'remote', 'ws']):
for service in services:
if hasattr(robot, service):
s = Thread(target=getattr(robot, service).run,
name='{}_server'.format(service))
s.daemon = True
s.start()
logger.info("Starting {} service".format(service))
@classmethod
def setup(cls, robot):
""" Classmethod used to specify your poppy creature.
This is where you should attach any specific primitives for instance.
"""
pass
@classproperty
@classmethod
def default_config(cls):
creature = camelcase_to_underscore(cls.__name__)
base_path = os.path.dirname(__import__(creature).__file__)
default_config = os.path.join(os.path.join(base_path, 'configuration'),
'{}.json'.format(creature))
with open(default_config) as f:
return json.load(f)

84
pypot-master/pypot-master/pypot/creatures/configure_utility.py Normal file
View File

@ -0,0 +1,84 @@
#!/usr/bin/env python
"""
Poppy configuration tools
Examples:
* poppy-configure ergo-jr m2
"""
import sys
from argparse import ArgumentParser, ArgumentDefaultsHelpFormatter
from subprocess import call
from pypot.creatures import installed_poppy_creatures
from pypot.utils import flushed_print as print
def find_port_for_motor(config, motor):
def flatten_motorgroups(motors, groups):
l = []
for m in motors:
if m in groups:
l += flatten_motorgroups(groups[m], groups)
else:
l.append(m)
return l
for bus in config['controllers']:
motors_on_bus = config['controllers'][bus]["attached_motors"]
motors = flatten_motorgroups(motors_on_bus, config['motorgroups'])
if motor in motors:
return config['controllers'][bus]["port"]
raise ValueError('Something must be wrong in your configuration file. '
'Could not find bus for motor {}'.format(motor))
def main():
robots = [c.replace('poppy-', '') for c in installed_poppy_creatures]
parser = ArgumentParser(description='Configuration tool for Poppy robots ',
formatter_class=ArgumentDefaultsHelpFormatter)
parser.add_argument('robot', type=str, choices=robots,
help='Robot used.')
parser.add_argument('motor', type=str,
help='Name of the motor to configure.')
args = parser.parse_args()
RobotCls = installed_poppy_creatures['poppy-{}'.format(args.robot)]
c = RobotCls.default_config
if args.motor not in c['motors']:
print('"{}" is not a motor of "{}"! '
'possibilities={}'.format(args.motor, args.robot,
sorted(c['motors'].keys())))
print('Exiting now...')
sys.exit(1)
motor_config = c['motors'][args.motor]
args = [
'--id', motor_config['id'],
'--type', motor_config['type'],
'--port', find_port_for_motor(c, args.motor),
'--return-delay-time', 0
]
if 'wheel_mode' in motor_config.keys():
args.extend(('--wheel-mode', motor_config['wheel_mode']))
else:
args.extend(('--angle-limit',motor_config['angle_limit'][0],motor_config['angle_limit'][1],
'--goto-zero'))
call(['dxl-config'] + list(map(str, args)))
if __name__ == '__main__':
main()

170
pypot-master/pypot-master/pypot/creatures/ik.py Normal file
View File

@ -0,0 +1,170 @@
from ikpy.chain import Chain
from ikpy.utils.geometry import rpy_matrix
from ikpy.urdf.URDF import get_chain_from_joints
from numpy import deg2rad, rad2deg, array, arctan2, sqrt
class IKChain(Chain):
""" Motors chain used for forward and inverse kinematics.
This class is based on the IK Chain as defined in the IKPY library (https://github.com/Phylliade/ikpy). It
provides convenient methods to directly create such a chain directly from a Poppy Creature.
"""
@classmethod
def from_poppy_creature(cls, poppy, motors, passiv, tip, reversed_motors=[]):
""" Creates an kinematic chain from motors of a Poppy Creature.
:param poppy: PoppyCreature used
:param list motors: list of all motors that composed the kinematic chain
:param list passiv: list of motors which are passiv in the chain (they will not move)
:param list tip: [x, y, z] translation of the tip of the chain (in meters)
:param list reversed_motors: list of motors that should be manually reversed (due to a problem in the URDF?)
"""
chain_elements = get_chain_from_joints(poppy.urdf_file, [m.name for m in motors])
activ = [False] + [m not in passiv for m in motors] + [True]
chain = cls.from_urdf_file(poppy.urdf_file,
base_elements=chain_elements,
last_link_vector=tip,
active_links_mask=activ)
chain.motors = [getattr(poppy, l.name) for l in chain.links[1:-1]]
for m, l in zip(chain.motors, chain.links[1:-1]):
# Force an access to angle limit to retrieve real values
# This is quite an ugly fix and should be handled better
m.angle_limit
bounds = m.__dict__['lower_limit'], m.__dict__['upper_limit']
l.bounds = tuple(map(rad2deg, bounds))
chain._reversed = array([(-1 if m in reversed_motors else 1) for m in motors])
return chain
@property
def joints_position(self):
""" Returns the joints position of all motors in the chain (in degrees). """
return [m.present_position for m in self.motors]
# Transformation matrix M:
# [[ Rx.x, Ry.x, Rz.x, T.x ], R = M[:3][:3] is the rotation matrix.
# [ Rx.y, Ry.y, Rz.y, T.y ],
# [ Rx.z, Ry.z, Rz.z, T.z ], T = M[:3][3] is the translation matrix. It corresponds
# [ 0 , 0 , 0, 1 ]] to the absolute coordinates of the effector
@property
def position(self):
""" Returns the cartesian position of the end of the chain (in meters). """
angles = self.convert_to_ik_angles(self.joints_position)
return self.forward_kinematics(angles)[:3, 3]
@property
def orientation(self):
""" Returns the rotation matrix along X axis (values from -1 to 1). """
angles = self.convert_to_ik_angles(self.joints_position)
return self.forward_kinematics(angles)[:3, 0]
@property
def pose(self):
"""
Gives the 4x4 affine transformation matrix of the current position
*Used for debug*
:return: 4x4 affine transformation matrix (float)
"""
angles = self.convert_to_ik_angles(self.joints_position)
return self.forward_kinematics(angles)
@property
def rpy(self):
"""
Gives the rpy values of the current position
:return: roll, pitch, yaw (float)
"""
angles = self.convert_to_ik_angles(self.joints_position)
R = self.forward_kinematics(angles)
yaw = arctan2(R[2][1], R[1][1])
pitch = arctan2(-R[3][1], sqrt(R[3][2] ^ 2 + R[3][3] ^ 2))
roll = arctan2(R[3][2], R[3][3])
return roll, pitch, yaw
def rpy_to_rotation_matrix(self, r, p, y):
"""
converts rpy to a 3x3 rotation matrix
:param r: roll (float)
:param p: pitch (float)
:param y: yaw (float)
:return: 3x3 rotation matrix
"""
return rpy_matrix(r, p, y)
def goto(self, position, orientation, duration, wait=False, accurate=False):
""" Goes to a given cartesian position.
:param list position: [x, y, z] representing the target position (in meters)
:param list orientation: [Rx.x, Rx.y, Rx.z] transformation along X axis (values from -1 to 1)
:param float duration: move duration
:param bool wait: whether to wait for the end of the move
:param bool accurate: trade-off between accurate solution and computation time. By default, use the not so
accurate but fast version.
"""
# if len(position) != 3:
# raise ValueError('Position should be a list [x, y, z]!')
self._goto(position, orientation, duration, wait, accurate)
def _goto(self, position, orientation, duration, wait, accurate):
""" Goes to a given cartesian pose.
:param matrix position: [x, y, z] representing the target position (in meters)
:param list orientation: [Rx.x, Rx.y, Rx.z] transformation along X axis (values from -1 to 1)
:param float duration: move duration
:param bool wait: whether to wait for the end of the move
:param bool accurate: trade-off between accurate solution and computation time. By default, use the not so
accurate but fast version.
"""
kwargs = {}
if not accurate:
kwargs['max_iter'] = 3
if orientation is not None:
shape = array(orientation).shape
if shape == (3, 3):
orientation_mode = "all"
elif shape == (3,):
orientation_mode = "X"
else:
orientation_mode = None
else:
orientation_mode = None
# q0 = self.convert_to_ik_angles(self.joints_position)
q = self.inverse_kinematics(target_position=position,
target_orientation=orientation,
orientation_mode=orientation_mode,
**kwargs)
joints = self.convert_from_ik_angles(q)
last = self.motors[-1]
for m, pos in list(zip(self.motors, joints)):
m.goto_position(pos, duration, wait=False if m != last else wait)
def convert_to_ik_angles(self, joints):
""" Convert from poppy representation to IKPY internal representation. """
if len(joints) != len(self.motors):
raise ValueError('Incompatible data, len(joints) should be {}!'.format(len(self.motors)))
raw_joints = [(j + m.offset) * (1 if m.direct else -1) for j, m in zip(joints, self.motors)]
raw_joints *= self._reversed
return [0] + [deg2rad(j) for j in raw_joints] + [0]
def convert_from_ik_angles(self, joints):
""" Convert from IKPY internal representation to poppy representation. """
if len(joints) != len(self.motors) + 2:
raise ValueError('Incompatible data, len(joints) should be {}!'.format(len(self.motors) + 2))
joints = [rad2deg(j) for j in joints[1:-1]]
joints *= self._reversed
return [(j * (1 if m.direct else -1)) - m.offset
for j, m in zip(joints, self.motors)]

280
pypot-master/pypot-master/pypot/creatures/services_launcher.py Normal file
View File

@ -0,0 +1,280 @@
#!/usr/bin/env python
import os
import sys
import time
import random
import logging
import argparse
import webbrowser
from contextlib import closing
from argparse import RawTextHelpFormatter
from pypot.server.snap import find_local_ip
from pypot.creatures import installed_poppy_creatures
from pypot.utils import flushed_print as print
from multiprocessing import Process
from http.server import HTTPServer
from http.server import SimpleHTTPRequestHandler
def start_poppy_with_services(args):
params = poppy_params_from_args(args)
for i in range(5):
try:
print('Attempt {} to start the robot...'.format(i + 1))
return installed_poppy_creatures[args.creature](**params)
except Exception as e:
# In case of failure,
# Give the robot some time to statup, reboot...
time.sleep(random.random())
print(e)
else:
print('Could not start up the robot...')
sys.exit(1)
def poppy_params_from_args(args):
params = {
'use_snap': args.snap,
'snap_port': args.snap_port,
'use_http': args.http or args.scratch,
'http_port': args.http_port,
'use_scratch': args.scratch,
'scratch_port': args.scratch_port,
'use_remote': args.remote,
'use_ws': args.ws,
'ws_port': args.ws_port,
}
if args.verbose:
params['snap_quiet'] = False
params['http_quiet'] = False
params['ws_quiet'] = False
if args.vrep:
params['simulator'] = 'vrep'
elif args.poppy_simu:
params['simulator'] = 'poppy-simu'
elif args.dummy:
params['simulator'] = 'dummy'
if args.disable_camera:
params['camera'] = 'dummy'
return params
def main():
parser = argparse.ArgumentParser(
description=('Poppy services launcher. Use it to quickly instantiate a poppy creature with Snap!, '
'Scratch, an http server, or a remote robot.'),
epilog="""
Examples:
* poppy-services --http poppy-ergo-jr
* poppy-services --snap poppy-torso
* poppy-services --scratch --vrep poppy-humanoid""",
formatter_class=RawTextHelpFormatter)
parser.add_argument('creature', type=str,
help='poppy creature name',
action='store', nargs='?',
choices=list(installed_poppy_creatures.keys()))
parser.add_argument('--dummy',
help='use a simulated dummy robot',
action='store_true')
parser.add_argument('--vrep',
help='use a V-REP simulated Poppy Creature',
action='store_true')
parser.add_argument('--poppy-simu',
help='start a simulated dummy robot and the HTTP API to connect to the viewer on simu.poppy-project.org',
action='store_true')
parser.add_argument('--snap',
help='start a Snap! robot server',
action='store_true')
parser.add_argument('--snap-port',
help='port used by the Snap! server',
default=6969, type=int)
parser.add_argument('--scratch',
help='start a Scratch robot server',
action='store_true')
parser.add_argument('--scratch-port',
help='port used by the Scratch server',
default=8601, type=int)
parser.add_argument('-nb', '--no-browser',
help='avoid automatic start of Snap! in web browser',
action='store_true')
parser.add_argument('--http',
help='start a http robot server',
action='store_true')
parser.add_argument('--http-port',
help='port of HttpRobotServer, used for poppy-simu',
default=8080, type=int)
parser.add_argument('--remote',
help='start a remote robot server',
action='store_true')
parser.add_argument('--ws',
help='start the websocket server',
action='store_true')
parser.add_argument('--ws-port',
help='port of Websocket Server',
default=9009, type=int)
parser.add_argument('--disable-camera',
help='Start the robot without the camera.',
action='store_true')
parser.add_argument('-v', '--verbose',
help='start services with verbose mode. There is 3 debug levels, add as "v" as debug level you want',
action='count')
parser.add_argument('-f', '--log-file',
help='Log filename',
action='store')
nb_creatures = len(installed_poppy_creatures.keys())
if nb_creatures == 0:
print('No installed poppy creature were found!')
print('You should first install the python package '
'corresponding to your robot or check your python environment.')
sys.exit(1)
args = parser.parse_args()
# If no creature are specified and only one is installed
# We use it as default.
if args.creature is None:
if nb_creatures > 1:
parser.print_help()
sys.exit(1)
args.creature = list(installed_poppy_creatures.keys())[0]
print('No creature specified, use {}'.format(args.creature))
if args.log_file:
fh = logging.FileHandler(args.log_file)
fh.setLevel(logging.DEBUG)
formatter = logging.Formatter(
'%(asctime)s - %(name)s - %(levelname)s - %(message)s')
fh.setFormatter(formatter)
logging.getLogger('').addHandler(fh)
if args.verbose:
args.snap_quiet = False
args.http_quiet = False
args.ws_quiet = False
if args.verbose == 1:
lvl = logging.WARNING
elif args.verbose == 2:
lvl = logging.INFO
elif args.verbose > 2:
lvl = logging.DEBUG
if args.log_file is not None:
ch = logging.FileHandler(args.log_file)
else:
ch = logging.StreamHandler()
ch.setLevel(lvl)
formatter = logging.Formatter(
'%(name)-12s: %(levelname)-8s %(message)s')
ch.setFormatter(formatter)
logging.getLogger('').addHandler(ch)
if not any([args.snap, args.scratch, args.http, args.remote, args.poppy_simu, args.ws, args.dummy]):
print('No service specified! See --help for details.')
sys.exit(1)
scratch_static_server_started = False
if args.scratch and not args.no_browser:
scratch_static_port = args.scratch_port
scratch_static_server = HTTPServer(("0.0.0.0", scratch_static_port), SimpleHTTPRequestHandler)
from pypot.vpl.scratch import download_scratch_interactively
static_app = download_scratch_interactively()
if static_app is None:
print("The static server was not started because the VPL app has not been downloaded")
else:
os.chdir(static_app)
scratch_static_server_process = Process(target=scratch_static_server.serve_forever, args=())
scratch_static_server_started = True
scratch_static_server_process.start()
scratch_url = 'http://127.0.0.1:{}/'.format(scratch_static_port)
url = '{}'.format(scratch_url)
snap_static_server_started = False
if args.snap and not args.no_browser:
snap_static_port = 8888
snap_static_server = HTTPServer(("0.0.0.0", snap_static_port), SimpleHTTPRequestHandler)
from pypot.vpl.snap import download_snap_interactively
static_app = download_snap_interactively()
if static_app is None:
print("The static server was not started because the VPL app has not been downloaded")
else:
os.chdir(static_app)
snap_static_server_process = Process(target=snap_static_server.serve_forever, args=())
snap_static_server_started = True
snap_static_server_process.start()
snap_url = 'http://127.0.0.1:{}/snap.html'.format(snap_static_port)
block_url = 'http://{}:{}/snap-blocks.xml'.format(
find_local_ip(), args.snap_port)
url = '{}#open:{}'.format(snap_url, block_url)
with closing(start_poppy_with_services(args)):
msg = ''
if args.dummy or args.poppy_simu:
msg += 'Simulated robot created! He is running on: ip={}'.format(find_local_ip())
else:
msg += 'Robot instantiated! He is running on: ip={},'.format(find_local_ip())
if args.disable_camera:
msg += ' without camera access.'
else:
msg += ' with camera access.'
if args.vrep: msg += ' With V-REP link.'
if args.snap or args.scratch or args.ws or args.http or args.poppy_simu:
msg += '\nServer started on:'
if args.http or args.scratch or args.poppy_simu: msg += ' http_port={},'.format(args.http_port)
if args.snap: msg += ' Snap_port={},'.format(args.snap_port)
if args.ws: msg += ' ws_port={},'.format(args.ws_port)
msg = msg[0:-1] + '.'
print(msg)
sys.stdout.flush()
if snap_static_server_started or scratch_static_server_started:
for browser_name in ['firefox', 'chromium-browser', 'chromium', 'google-chrome',
'chrome', 'safari', 'midori', None]:
try:
browser = webbrowser.get(browser_name)
browser.open(url, new=0, autoraise=True)
break
except Exception:
pass
# Just run4ever (until Ctrl-c...)
try:
while (True):
time.sleep(1000)
except KeyboardInterrupt:
print("Bye bye!")
if scratch_static_server_started:
scratch_static_server_process.terminate()
scratch_static_server_process.join()
if snap_static_server_started:
snap_static_server_process.terminate()
snap_static_server_process.join()
if __name__ == '__main__':
main()

141
pypot-master/pypot-master/pypot/dynamixel/__init__.py Normal file
View File

@ -0,0 +1,141 @@
import sys
import platform
import glob
import logging
import serial.tools.list_ports
from .io import DxlIO, Dxl320IO, DxlXM430IO, DxlXL330IO
from .error import BaseErrorHandler
from .syncloop import BaseDxlController
from .motor import DxlMXMotor, DxlAXRXMotor, DxlXL320Motor, DxlSRMotor
from .io.abstract_io import DxlError
from ..robot import Robot
logger = logging.getLogger(__name__)
def _get_available_ports():
""" Tries to find the available serial ports on your system. """
if platform.system() == 'Darwin':
return glob.glob('/dev/tty.usb*')
elif platform.system() == 'Linux':
return glob.glob('/dev/ttyACM*') + glob.glob('/dev/ttyUSB*') + glob.glob('/dev/ttyAMA*')
elif sys.platform.lower() == 'cygwin':
return glob.glob('/dev/com*')
elif platform.system() == 'Windows':
import winreg
import itertools
ports = []
path = 'HARDWARE\\DEVICEMAP\\SERIALCOMM'
key = winreg.OpenKey(winreg.HKEY_LOCAL_MACHINE, path)
for i in itertools.count():
try:
ports.append(str(winreg.EnumValue(key, i)[1]))
except WindowsError:
return ports
else:
raise EnvironmentError('{} is an unsupported platform, cannot find serial ports!'.format(platform.system()))
return []
def get_available_ports(only_free=False):
ports = _get_available_ports()
if only_free:
ports = [port for port in ports if port not in DxlIO.get_used_ports()]
return ports
def get_port_vendor_info(port=None):
""" Return vendor informations of a usb2serial device.
It may depends on the Operating System.
:param string port: port of the usb2serial device
:Example:
Result with a USB2Dynamixel on Linux:
In [1]: import pypot.dynamixel
In [2]: pypot.dynamixel.get_port_vendor_info('/dev/ttyUSB0')
Out[2]: 'USB VID:PID=0403:6001 SNR=A7005LKE' """
port_info_dict = dict((x[0], x[2]) for x in serial.tools.list_ports.comports())
return port_info_dict[port] if port is not None else port_info_dict
def find_port(ids, strict=True):
""" Find the port with the specified attached motor ids.
:param list ids: list of motor ids to find
:param bool strict: specify if all ids should be find (when set to False, only half motor must be found)
.. warning:: If two (or more) ports are attached to the same list of motor ids the first match will be returned.
"""
ids_founds = []
for port in get_available_ports():
for DxlIOCls in (DxlIO, Dxl320IO):
try:
with DxlIOCls(port) as dxl:
_ids_founds = dxl.scan(ids)
ids_founds += _ids_founds
if strict and sorted(_ids_founds) == sorted(ids):
return port
if not strict and len(_ids_founds) >= len(ids) / 2:
logger.warning('Missing ids: {}'.format(ids, list(set(ids) - set(_ids_founds))))
return port
if len(ids_founds) > 0:
logger.warning('Port:{} ids found:{}'.format(port, _ids_founds))
except DxlError:
logger.warning('DxlError on port {}'.format(port))
continue
missing = list(set(ids) - set(ids_founds))
if len(missing) == 0:
raise ValueError('All motors have been found but they are not connected as specified in the configuration file, please check connections')
raise IndexError('No suitable port found for ids {}. These ids are missing {} !'.format(ids, missing))
def autodetect_robot():
""" Creates a :class:`~pypot.robot.robot.Robot` by detecting dynamixel motors on all available ports. """
motor_controllers = []
for port in get_available_ports():
for DxlIOCls in (DxlIO, Dxl320IO):
dxl_io = DxlIOCls(port)
ids = dxl_io.scan()
if not ids:
dxl_io.close()
continue
models = dxl_io.get_model(ids)
motorcls = {
'MX': DxlMXMotor,
'RX': DxlAXRXMotor,
'AX': DxlAXRXMotor,
'XL': DxlXL320Motor,
'SR': DxlSRMotor,
}
motors = [motorcls[model[:2]](id, model=model)
for id, model in zip(ids, models)]
c = BaseDxlController(dxl_io, motors)
motor_controllers.append(c)
break
return Robot(motor_controllers)

210
pypot-master/pypot-master/pypot/dynamixel/controller.py Normal file
View File

@ -0,0 +1,210 @@
import time
import logging
from .io.abstract_io import DxlError
from ..robot.controller import MotorsController
logger = logging.getLogger(__name__)
class DxlController(MotorsController):
def __init__(self, io, motors, sync_freq, synchronous,
mode, regname, varname=None):
MotorsController.__init__(self, io, motors, sync_freq)
self.ids = [m.id for m in self.working_motors]
self.synchronous = synchronous
self.mode = mode
self.regname = regname
self.varname = regname if varname is None else varname
for m in motors:
if mode == 'get':
m._read_synchronous[self.varname] = self.synchronous
else:
m._write_synchronous[self.varname] = self.synchronous
@property
def working_motors(self):
return [m for m in self.motors if not m._broken]
@property
def synced_motors(self):
motors = [m for m in self.working_motors if self.varname in m.registers]
if self.synchronous:
motors = ([m for m in motors if m._read_synced[self.varname].needed]
if self.mode == 'get' else
[m for m in motors if m._write_synced[self.varname].needed])
return motors
def setup(self):
if self.mode == 'set':
MAX_TRIALS = 25
for _ in range(MAX_TRIALS):
if self.get_register(self.working_motors, disable_sync_read=True):
break
time.sleep(0.1)
else:
raise IOError('Cannot initialize syncloop for "{}". You need to desactivate sync_read if you use a usb2dynamixel device. '.format(
self.regname))
def update(self):
if not self.synced_motors:
return
return (self.get_register(self.synced_motors)
if self.mode == 'get' else
self.set_register(self.synced_motors))
def get_register(self, motors, disable_sync_read=False):
""" Gets the value from the specified register and sets it to the :class:`~pypot.dynamixel.motor.DxlMotor`. """
if not motors:
return False
ids = [m.id for m in motors]
getter = getattr(self.io, 'get_{}'.format(self.regname))
values = (sum([list(getter([id])) for id in ids], [])
if disable_sync_read else
getter(ids))
if not values:
return False
for m, val in zip(motors, values):
m.__dict__[self.varname] = val
for m in motors:
m._read_synced[self.varname].done()
return True
def set_register(self, motors):
""" Gets the value from :class:`~pypot.dynamixel.motor.DxlMotor` and sets it to the specified register. """
if not motors:
return
ids = [m.id for m in motors]
values = (m.__dict__[self.varname] for m in motors)
getattr(self.io, 'set_{}'.format(self.regname))(dict(zip(ids, values)))
for m in motors:
m._write_synced[self.varname].done()
class AngleLimitRegisterController(DxlController):
def __init__(self, io, motors, sync_freq, synchronous):
DxlController.__init__(self, io, motors, sync_freq,
synchronous, 'get', 'angle_limit')
self.varnames = ['lower_limit', 'upper_limit']
for m in motors:
for var in self.varnames:
m._read_synchronous[var] = self.synchronous
@property
def synced_motors(self):
motors = self.working_motors
if self.synchronous:
sync_motors = []
for m in motors:
for var in self.varnames:
if m._read_synced[var].needed:
sync_motors.append(m)
motors = sync_motors
return motors
def get_register(self, motors):
if not motors:
return
ids = [m.id for m in motors]
values = self.io.get_angle_limit(ids)
for m, val in zip(motors, values):
m.__dict__['lower_limit'], m.__dict__['upper_limit'] = val
for m in motors:
for var in ['lower_limit', 'upper_limit']:
m._read_synced[var].done()
class PosSpeedLoadDxlController(DxlController):
def __init__(self, io, motors, sync_freq):
DxlController.__init__(self, io, motors, sync_freq,
False, 'get', 'present_position')
def setup(self):
torques = self.io.is_torque_enabled(self.ids)
for m, c in zip(self.working_motors, torques):
m.compliant = not c
self._old_torques = torques
self._old_goals = {m.id: 0.0 for m in self.motors}
try:
values = self.io.get_goal_position_speed_load(self.ids)
positions, speeds, loads = zip(*values)
except ValueError:
raise DxlError("Couldn't initialize pos/speed/load sync loop!")
for m, p, s, l in zip(self.working_motors, positions, speeds, loads):
m.__dict__['goal_position'] = p
m.__dict__['moving_speed'] = s
m.__dict__['torque_limit'] = l
def update(self):
self.get_present_position_speed_load(self.working_motors)
self.set_goal_position_speed_load(self.working_motors)
def get_present_position_speed_load(self, motors):
ids = [m.id for m in motors]
values = self.io.get_present_position_speed_load(ids)
if not values:
logger.warning('Timeout when getting pos/speed/load from %s', ids)
return
positions, speeds, loads = zip(*values)
for m, p, s, l in zip(motors, positions, speeds, loads):
m.__dict__['present_position'] = p
m.__dict__['present_speed'] = s
m.__dict__['present_load'] = l
def set_goal_position_speed_load(self, motors):
change_torque = {}
torques = [not m.compliant for m in motors]
for m, t, old_t in zip(motors, torques, self._old_torques):
if t != old_t:
change_torque[m.id] = t
self._old_torques = torques
if change_torque:
self.io._set_torque_enable(change_torque)
rigid_motors = []
for m in motors:
# Filter force control motors - only update values if goal_position has changed
if getattr(m, "force_control_enable", False) and not m.compliant and self._old_goals[m.id] != m.__dict__['goal_position']:
rigid_motors += [m]
self._old_goals[m.id] = m.__dict__['goal_position']
# Do not filter motors without force control
elif not m.compliant:
rigid_motors += [m]
ids = tuple(m.id for m in rigid_motors)
if not ids:
return
values = ((m.__dict__['goal_position'],
m.__dict__['moving_speed'],
m.__dict__['torque_limit']) for m in rigid_motors)
self.io.set_goal_position_speed_load(dict(zip(ids, values)))

421
pypot-master/pypot-master/pypot/dynamixel/conversion.py Normal file
View File

@ -0,0 +1,421 @@
# -*- coding: utf-8 -*-
"""
This module describes all the conversion method used to transform value from the representation used by the dynamixel motor to a more standard form (e.g. degrees, volt...).
For compatibility issue all comparison method should be written in the following form (even if the model is not actually used):
* def my_conversion_from_dxl_to_si(value, model): ...
* def my_conversion_from_si_to_dxl(value, model): ...
.. note:: If the control is readonly you only need to write the dxl_to_si conversion.
"""
import numpy
import itertools
from enum import Enum
# MARK: - Position
position_range = {
'MX': (4096, 360.0),
'SR': (4096, 360.0),
'EX': (4096, 251.0),
'*': (1024, 300.0),
'XM': (4096, 360.0)
}
torque_max = { # in N.m
'MX-106': 8.4,
'MX-64': 6.0,
'MX-28': 2.5,
'MX-12': 1.2,
'AX-12': 1.2,
'AX-18': 1.8,
'RX-24': 2.6,
'RX-28': 2.5,
'RX-64': 4.0,
'XL-320': 0.39,
'SR-RH4D': 0.57,
'EX-106': 10.9
}
velocity = { # in degree/s
'MX-106': 270.,
'MX-64': 378.,
'MX-28': 330.,
'MX-12': 2820.,
'AX-12': 354.,
'AX-18': 582.,
'RX-24': 756.,
'RX-28': 402.,
'RX-64': 294.,
'SR-RH4D': 300.0,
}
def dxl_to_degree(value, model):
determined_model = '*'
if model.startswith('MX'):
determined_model = 'MX'
elif model.startswith('SR'):
determined_model = 'SR'
elif model.startswith('EX'):
determined_model = 'EX'
elif model.startswith('XM'):
determined_model = 'XM'
max_pos, max_deg = position_range[determined_model]
return round(((max_deg * float(value)) / (max_pos - 1)) - (max_deg / 2), 2)
def degree_to_dxl(value, model):
determined_model = '*'
if model.startswith('MX'):
determined_model = 'MX'
elif model.startswith('SR'):
determined_model = 'SR'
elif model.startswith('EX'):
determined_model = 'EX'
elif model.startswith('XM'):
determined_model = 'XM'
max_pos, max_deg = position_range[determined_model]
pos = int(round((max_pos - 1) * ((max_deg / 2 + float(value)) / max_deg), 0))
pos = min(max(pos, 0), max_pos - 1)
return pos
# MARK: - Speed
# Speed factor (RPM per least significant bit)
def _speed_factor(model):
if model == 'MX-12':
return 0.916
if model.startswith('MX') or model.startswith('SR'):
return 0.114
return 0.111
def dxl_to_speed(value, model):
cw, speed = divmod(value, 1024)
direction = (-2 * cw + 1)
return direction * (speed * _speed_factor(model)) * 6
def speed_to_dxl(value, model):
direction = 1024 if value < 0 else 0
speed_factor = _speed_factor(model)
max_value = 1023 * speed_factor * 6
value = min(max(value, -max_value), max_value)
return int(round(direction + abs(value) / (6 * speed_factor), 0))
# MARK: - Torque
def dxl_to_torque(value, model):
return round(value / 10.23, 1)
def torque_to_dxl(value, model):
return int(round(value * 10.23, 0))
def dxl_to_load(value, model):
cw, load = divmod(value, 1024)
direction = -2 * cw + 1
return dxl_to_torque(load, model) * direction
# MARK - Acceleration
def dxl_to_acceleration(value, model):
"""Converts from ticks to degress/second^2"""
return value * 8.583 # degrees / sec**2
def acceleration_to_dxl(value, model):
"""Converts from degrees/second^2 to ticks"""
return int(round(value / 8.583, 0)) # degrees / sec**2
# PID Gains
def dxl_to_pid(value, model):
return (value[0] * 0.004,
value[1] * 0.48828125,
value[2] * 0.125)
def pid_to_dxl(value, model):
def truncate(x):
return int(max(0, min(x, 254)))
return [truncate(x * y) for x, y in zip(value, (250, 2.048, 8.0))]
# MARK: - Model
dynamixelModels = {
12: 'AX-12', # 12 + (0<<8)
18: 'AX-18', # 18 + (0<<8)
24: 'RX-24', # 24 + (0<<8)
28: 'RX-28', # 28 + (0<<8)
29: 'MX-28', # 29 + (0<<8)
64: 'RX-64', # 64 + (0<<8)
107: 'EX-106',
360: 'MX-12', # 104 + (1<<8)
310: 'MX-64', # 54 + (1<<8)
320: 'MX-106', # 64 + (1<<8)
350: 'XL-320', # 94 + (1<<8)
400: 'SR-RH4D',
401: 'SR-RH4D', # Virtual motor
16897: 'USB2AX',
1030: 'XM-430',
}
def dxl_to_model(value, dummy=None):
try:
return dynamixelModels[value]
except KeyError:
return 'Unknown model number {}'.format(value)
# MARK: - Drive Mode
def check_bit(value, offset):
return bool(value & (1 << offset))
def dxl_to_drive_mode(value, model):
return ('reverse' if check_bit(value, 0) else 'normal',
'slave' if check_bit(value, 1) else 'master')
def drive_mode_to_dxl(value, model):
return (int('slave' in value) << 1 | int('reverse' in value))
# MARK: - Baudrate
dynamixelBaudrates = {
1: 1000000.0,
3: 500000.0,
4: 400000.0,
7: 250000.0,
9: 200000.0,
16: 117647.1,
34: 57600.0,
103: 19230.8,
207: 9615.4,
250: 2250000.0,
251: 2500000.0,
252: 3000000.0,
}
dynamixelBaudratesWithModel = {
'XL-320': {
0: 9600.0,
1: 57600.0,
2: 115200.0,
3: 1000000.0,
4: 2000000.0
}
}
def dxl_to_baudrate(value, model):
return dynamixelBaudratesWithModel.get(model, dynamixelBaudrates)[value]
def baudrate_to_dxl(value, model):
current_baudrates = dynamixelBaudratesWithModel.get(model, dynamixelBaudrates)
for k, v in current_baudrates.items():
if (abs(v - value) / float(value)) < 0.05:
return k
raise ValueError('incorrect baudrate {} (possible values {})'.format(value, list(current_baudrates.values())))
# MARK: - Return Delay Time
def dxl_to_rdt(value, model):
return value * 2
def rdt_to_dxl(value, model):
return int(value / 2)
# MARK: - Temperature
def dxl_to_temperature(value, model):
return float(value)
def temperature_to_dxl(value, model):
return int(value)
# MARK: - Current
def dxl_to_current(value, model):
if model.startswith('SR'):
# The SR motors do use a different conversion formula than the dynamixel motors
# See http://kb.seedrobotics.com/doku.php?id=dh4d:dynamixelcontroltables
return (value * 0.4889) / 1000.0
else:
return 4.5 * (value - 2048.0) / 1000.0
# MARK: - Voltage
def dxl_to_voltage(value, model):
return value * 0.1
def voltage_to_dxl(value, model):
return int(value * 10)
# MARK: - Status Return Level
status_level = ('never', 'read', 'always')
def dxl_to_status(value, model):
return status_level[value]
def status_to_dxl(value, model):
if value not in status_level:
raise ValueError('status "{}" should be chosen among {}'.format(value, status_level))
return status_level.index(value)
# MARK: - Error
# TODO: depend on protocol v1 vs v2
dynamixelErrors = ['None Error',
'Instruction Error',
'Overload Error',
'Checksum Error',
'Range Error',
'Overheating Error',
'Angle Limit Error',
'Input Voltage Error']
def dxl_to_alarm(value, model):
return decode_error(value)
def decode_error(error_code):
bits = numpy.unpackbits(numpy.asarray(error_code, dtype=numpy.uint8))
return tuple(numpy.array(dynamixelErrors)[bits == 1])
def alarm_to_dxl(value, model):
if not set(value).issubset(dynamixelErrors):
raise ValueError('should only contains error among {}'.format(dynamixelErrors))
indices = [len(dynamixelErrors) - 1 - dynamixelErrors.index(e) for e in value]
return sum(2 ** i for i in indices)
XL320LEDColors = Enum('Colors', 'off red green yellow '
'blue pink cyan white')
def dxl_to_led_color(value, model):
return XL320LEDColors(value + 1).name
def led_color_to_dxl(value, model):
value = getattr(XL320LEDColors, value).value - 1
value = int(value) & 0b111
return value
control_modes = {
1: 'wheel',
2: 'joint',
}
def dxl_to_control_mode(value, _):
return control_modes[value]
def control_mode_to_dxl(mode, _):
return (next((v for v, m in control_modes.items()
if m == mode), None))
# MARK: - Various utility functions
def dxl_to_bool(value, model):
return bool(value)
def bool_to_dxl(value, model):
return int(value)
def dxl_decode(data):
if len(data) not in (1, 2, 4, 8):
raise ValueError('try to decode incorrect data {}'.format(data))
if len(data) == 1:
return data[0]
if len(data) == 2:
return data[0] + (data[1] << 8)
if len(data) == 4:
return int.from_bytes(data, byteorder='little', signed=False)
if len(data) == 8:
return (
data[0] + (data[1] << 8) + (data[2] << 16) + (data[3] << 24),
data[4] + (data[5] << 8) + (data[6] << 16) + (data[7] << 24),
)
def dxl_decode_all(data, nb_elem):
if nb_elem > 1:
data = list(zip(*([iter(data)] * (len(data) // nb_elem))))
return tuple(map(dxl_decode, data))
else:
return dxl_decode(data)
def dxl_code(value, length):
if length not in (1, 2, 4):
raise ValueError('try to code value with an incorrect length {}'.format(length))
if length == 1:
return (value, )
if length == 2:
return (value % 256, value >> 8)
if length == 4:
return value.to_bytes(4, byteorder='little', signed=False)
def dxl_code_all(value, length, nb_elem):
if nb_elem > 1:
return list(itertools.chain(*(dxl_code(v, length) for v in value)))
else:
return dxl_code(value, length)

74
pypot-master/pypot-master/pypot/dynamixel/error.py Normal file
View File

@ -0,0 +1,74 @@
# -*- coding: utf-8 -*-
import logging
logger = logging.getLogger(__name__)
class DxlErrorHandler(object):
""" This class is used to represent all the error that you can/should handle.
The errors can be of two types:
* communication error (timeout, communication)
* motor error (voltage, limit, overload...)
This class was designed as an abstract class and so you should write your own handler by subclassing this class and defining the apropriate behavior for your program.
.. warning:: The motor error should be overload carrefuly as they can indicate important mechanical issue.
"""
# MARK: - Communication errors
def handle_timeout(self, timeout_error):
raise NotImplementedError
def handle_communication_error(self, communication_error):
raise NotImplementedError
# MARK: - Motor errors
def handle_input_voltage_error(self, instruction_packet):
raise NotImplementedError
def handle_angle_limit_error(self, instruction_packet):
raise NotImplementedError
def handle_overheating_error(self, instruction_packet):
raise NotImplementedError
def handle_range_error(self, instruction_packet):
raise NotImplementedError
def handle_checksum_error(self, instruction_packet):
raise NotImplementedError
def handle_overload_error(self, instruction_packet):
raise NotImplementedError
def handle_instruction_error(self, instruction_packet):
raise NotImplementedError
def handle_none_error(self, instruction_packet):
raise NotImplementedError
class BaseErrorHandler(DxlErrorHandler):
""" This class is a basic handler that just skip the communication errors. """
def handle_timeout(self, timeout_error):
msg = 'Timeout after sending {} to motors {}'.format(timeout_error.instruction_packet,
timeout_error.ids)
logger.warning(msg,
extra={'port': timeout_error.dxl_io.port,
'baudrate': timeout_error.dxl_io.baudrate,
'timeout': timeout_error.dxl_io.timeout})
def handle_communication_error(self, com_error):
msg = 'Communication error after sending {}'.format(com_error.instruction_packet)
logger.warning(msg,
extra={'port': com_error.dxl_io.port,
'baudrate': com_error.dxl_io.baudrate,
'timeout': com_error.dxl_io.timeout})
def handle_none_error(self, instruction_packet):
logger.info('None Error!')

5
pypot-master/pypot-master/pypot/dynamixel/io/__init__.py Normal file
View File

@ -0,0 +1,5 @@
from .io import DxlIO
from .io_320 import Dxl320IO
from .io_xm430 import DxlXM430IO
from .io_xl330 import DxlXL330IO
from .abstract_io import DxlError

581
pypot-master/pypot-master/pypot/dynamixel/io/abstract_io.py Normal file
View File

@ -0,0 +1,581 @@
# -*- coding: utf-8 -*-
import serial
import logging
import operator
import itertools
import threading
from collections import namedtuple, OrderedDict
from contextlib import contextmanager
from ..conversion import (dxl_code_all, dxl_decode_all, decode_error,
dxl_to_model)
logger = logging.getLogger(__name__)
# With this logger you should always provide as extra:
# - the port
# - the baudrate
# - the timeout
_DxlControl = namedtuple('_DxlControl', ('name',
'address', 'length', 'nb_elem',
'access',
'models',
'dxl_to_si', 'si_to_dxl',
'getter_name', 'setter_name'))
class _DxlAccess(object):
readonly, writeonly, readwrite = range(3)
class AbstractDxlIO(object):
""" Low-level class to handle the serial communication with the robotis motors. """
__used_ports = set()
__controls = []
_protocol = None
@classmethod
def get_used_ports(cls):
return list(cls.__used_ports)
# MARK: - Open, Close and Flush the communication
def __init__(self,
port, baudrate=1000000, timeout=0.05,
use_sync_read=False,
error_handler_cls=None,
convert=True):
""" At instantiation, it opens the serial port and sets the communication parameters.
:param string port: the serial port to use (e.g. Unix (/dev/tty...), Windows (COM...)).
:param int baudrate: default for new motors: 57600, for PyPot motors: 1000000
:param float timeout: read timeout in seconds
:param bool use_sync_read: whether or not to use the SYNC_READ instruction
:param error_handler: set a handler that will receive the different errors
:type error_handler: :py:class:`~pypot.dynamixel.error.DxlErrorHandler`
:param bool convert: whether or not convert values to units expressed in the standard system
:raises: :py:exc:`~pypot.dynamixel.io.DxlError` if the port is already used.
"""
self._known_models = {}
self._known_mode = {}
self._sync_read = use_sync_read
self._error_handler = error_handler_cls() if error_handler_cls else None
self._convert = convert
self._serial_lock = threading.Lock()
self.open(port, baudrate, timeout)
def __enter__(self):
return self
def __del__(self):
self.close()
def __exit__(self, exc_type, exc_value, traceback):
self.close()
def __repr__(self):
return ('<DXL IO: closed={self.closed}, '
'port="{self.port}", '
'baudrate={self.baudrate}, '
'timeout={self.timeout}>').format(self=self)
def open(self, port, baudrate=1000000, timeout=0.05):
""" Opens a new serial communication (closes the previous communication if needed).
:raises: :py:exc:`~pypot.dynamixel.io.DxlError` if the port is already used.
"""
self._open(port, baudrate, timeout)
logger.info("Opening port '%s'", self.port,
extra={'port': port,
'baudrate': baudrate,
'timeout': timeout})
def _open(self, port, baudrate, timeout, max_recursion=500):
# Tries to connect to port until it succeeds to ping any motor on the bus.
# This is used to circumvent a bug with the driver for the USB2AX on Mac.
# Warning: If no motor is connected on the bus, this will run forever!!!
import platform
# import time
import pypot.utils.pypot_time as time
for i in range(max_recursion):
self._known_models.clear()
self._known_mode.clear()
with self._serial_lock:
self.close(_force_lock=True)
if port in self.__used_ports:
logging.warn('Another instance of pypot is using the port {}. This might be an issue in Jupyter Notebook, in that case restart the kernel.'.format(port))
# Dirty walkaround to fix a strange bug.
# Observed with the USB2AX on Linux with pyserial 2.7
# We have to first open/close the port in order to make it work
# at 1Mbauds
if platform.system() == 'Linux' and self._sync_read:
self._serial = serial.Serial(port, 9600)
self._serial.close()
self._serial = serial.Serial(port, baudrate, timeout=timeout, write_timeout=timeout)
self.__used_ports.add(port)
if (platform.system() == 'Darwin' and
self._protocol.name == 'v1' and self._sync_read):
if not self.ping(self._protocol.DxlBroadcast):
self.close()
continue
else:
time.sleep(self.timeout)
self.flush()
break
else:
raise DxlError('could not connect to the port {}'.format(self.port))
def close(self, _force_lock=False):
""" Closes the serial communication if opened. """
if not self.closed:
with self.__force_lock(_force_lock) or self._serial_lock:
self._serial.close()
if self.port in self.__used_ports: self.__used_ports.remove(self.port)
logger.info("Closing port '%s'", self.port,
extra={'port': self.port,
'baudrate': self.baudrate,
'timeout': self.timeout})
def flush(self, _force_lock=False):
""" Flushes the serial communication (both input and output). """
if self.closed:
raise DxlError('attempt to flush a closed serial communication')
with self.__force_lock(_force_lock) or self._serial_lock:
self._serial.flushInput()
self._serial.flushOutput()
def __force_lock(self, condition):
return with_True() if condition else False
# MARK: Properties of the serial communication
@property
def port(self):
""" Port used by the :class:`~pypot.dynamixel.io.DxlIO`. If set, will re-open a new connection. """
return self._serial.port
@port.setter
def port(self, value):
self.open(value, self.baudrate, self.timeout)
@property
def baudrate(self):
""" Baudrate used by the :class:`~pypot.dynamixel.io.DxlIO`. If set, will re-open a new connection. """
return self._serial.baudrate
@baudrate.setter
def baudrate(self, value):
self.open(self.port, value, self.timeout)
@property
def timeout(self):
""" Timeout used by the :class:`~pypot.dynamixel.io.DxlIO`. If set, will re-open a new connection. """
return self._serial.timeout
@timeout.setter
def timeout(self, value):
self.open(self.port, self.baudrate, value)
@property
def closed(self):
""" Checks if the connection is closed. """
return not (hasattr(self, '_serial') and self._serial.isOpen())
# MARK: - Motor discovery
def ping(self, id):
""" Pings the motor with the specified id.
.. note:: The motor id should always be included in [0, 253]. 254 is used for broadcast.
"""
pp = self._protocol.DxlPingPacket(id)
try:
self._send_packet(pp, error_handler=None)
return True
except DxlTimeoutError:
return False
def scan(self, ids=range(254)):
""" Pings all ids within the specified list, by default it finds all the motors connected to the bus. """
return [id for id in ids if self.ping(id)]
# MARK: - Specific Getter / Setter
def get_model(self, ids):
""" Gets the model for the specified motors. """
to_get_ids = [i for i in ids if i not in self._known_models]
models = [dxl_to_model(m) for m in self._get_model(to_get_ids, convert=False)]
self._known_models.update(zip(to_get_ids, models))
return tuple(self._known_models[id] for id in ids)
def change_id(self, new_id_for_id):
""" Changes the id of the specified motors (each id must be unique on the bus). """
if len(set(new_id_for_id.values())) < len(new_id_for_id):
raise ValueError('each id must be unique.')
for new_id in new_id_for_id.values():
if self.ping(new_id):
raise ValueError('id {} is already used.'.format(new_id))
self._change_id(new_id_for_id)
for motor_id, new_id in new_id_for_id.items():
if motor_id in self._known_models:
self._known_models[new_id] = self._known_models[motor_id]
del self._known_models[motor_id]
if motor_id in self._known_mode:
self._known_mode[new_id] = self._known_mode[motor_id]
del self._known_mode[motor_id]
def change_baudrate(self, baudrate_for_ids):
""" Changes the baudrate of the specified motors. """
self._change_baudrate(baudrate_for_ids)
for motor_id in baudrate_for_ids:
if motor_id in self._known_models:
del self._known_models[motor_id]
if motor_id in self._known_mode:
del self._known_mode[motor_id]
def get_status_return_level(self, ids, **kwargs):
""" Gets the status level for the specified motors. """
convert = kwargs['convert'] if 'convert' in kwargs else self._convert
srl = []
for id in ids:
try:
srl.extend(self._get_status_return_level((id, ),
error_handler=None, convert=convert))
except DxlTimeoutError as e:
if self.ping(id):
srl.append('never' if convert else 0)
else:
if self._error_handler:
self._error_handler.handle_timeout(e)
return ()
else:
raise e
return tuple(srl)
def set_status_return_level(self, srl_for_id, **kwargs):
""" Sets status return level to the specified motors. """
convert = kwargs['convert'] if 'convert' in kwargs else self._convert
if convert:
srl_for_id = dict(zip(srl_for_id.keys(),
[('never', 'read', 'always').index(s) for s in srl_for_id.values()]))
self._set_status_return_level(srl_for_id, convert=False)
def switch_led_on(self, ids):
""" Switches on the LED of the motors with the specified ids. """
self._set_LED(dict(zip(ids, itertools.repeat(True))))
def switch_led_off(self, ids):
""" Switches off the LED of the motors with the specified ids. """
self._set_LED(dict(zip(ids, itertools.repeat(False))))
def enable_torque(self, ids):
""" Enables torque of the motors with the specified ids. """
self._set_torque_enable(dict(zip(ids, itertools.repeat(True))))
def disable_torque(self, ids):
""" Disables torque of the motors with the specified ids. """
self._set_torque_enable(dict(zip(ids, itertools.repeat(False))))
def get_pid_gain(self, ids, **kwargs):
""" Gets the pid gain for the specified motors. """
return tuple([tuple(reversed(t)) for t in self._get_pid_gain(ids, **kwargs)])
def set_pid_gain(self, pid_for_id, **kwargs):
""" Sets the pid gain to the specified motors. """
pid_for_id = dict(zip(pid_for_id.keys(),
[tuple(reversed(t)) for t in pid_for_id.values()]))
self._set_pid_gain(pid_for_id, **kwargs)
# MARK: - Generic Getter / Setter
def get_control_table(self, ids, **kwargs):
""" Gets the full control table for the specified motors.
..note:: This function requires the model for each motor to be known. Querring this additional information might add some extra delay.
"""
error_handler = kwargs['error_handler'] if ('error_handler' in kwargs) else self._error_handler
convert = kwargs['convert'] if ('convert' in kwargs) else self._convert
bl = ('goal position speed load', 'present position speed load')
controls = [c for c in self._AbstractDxlIO__controls if c.name not in bl]
res = []
for id, model in zip(ids, self.get_model(ids)):
controls = [c for c in controls if model in c.models]
controls = sorted(controls, key=lambda c: c.address)
address = controls[0].address
length = controls[-1].address + controls[-1].nb_elem * controls[-1].length
rp = self._protocol.DxlReadDataPacket(id, address, length)
sp = self._send_packet(rp, error_handler=error_handler)
d = OrderedDict()
for c in controls:
v = dxl_decode_all(sp.parameters[c.address:c.address + c.nb_elem * c.length], c.nb_elem)
d[c.name] = c.dxl_to_si(v, model) if convert else v
res.append(d)
return tuple(res)
@classmethod
def _generate_accessors(cls, control):
cls.__controls.append(control)
if control.access in (_DxlAccess.readonly, _DxlAccess.readwrite):
def my_getter(self, ids, **kwargs):
return self._get_control_value(control, ids, **kwargs)
func_name = control.getter_name if control.getter_name else 'get_{}'.format(control.name.replace(' ', '_'))
func_name = '_{}'.format(func_name) if hasattr(cls, func_name) else func_name
my_getter.__doc__ = 'Gets {} from the specified motors.'.format(control.name)
my_getter.__name__ = func_name
setattr(cls, func_name, my_getter)
if control.access in (_DxlAccess.writeonly, _DxlAccess.readwrite):
def my_setter(self, value_for_id, **kwargs):
self._set_control_value(control, value_for_id, **kwargs)
func_name = control.setter_name if control.setter_name else 'set_{}'.format(control.name.replace(' ', '_'))
func_name = '_{}'.format(func_name) if hasattr(cls, func_name) else func_name
my_setter.__doc__ = 'Sets {} to the specified motors.'.format(control.name)
my_setter.__name__ = func_name
setattr(cls, func_name, my_setter)
def _get_control_value(self, control, ids, **kwargs):
if not ids:
return ()
error_handler = kwargs['error_handler'] if ('error_handler' in kwargs) else self._error_handler
convert = kwargs['convert'] if ('convert' in kwargs) else self._convert
if self._sync_read and len(ids) > 1:
rp = self._protocol.DxlSyncReadPacket(ids, control.address,
control.length * control.nb_elem)
with self._serial_lock:
sp = self._send_packet(rp,
error_handler=error_handler,
_force_lock=True)
if not sp:
return ()
if self._protocol.name == 'v1':
values = sp.parameters
elif self._protocol.name == 'v2':
values = list(sp.parameters)
for i in range(len(ids) - 1):
try:
sp = self.__real_read(rp, _force_lock=True)
except (DxlTimeoutError, DxlCommunicationError):
return ()
values.extend(sp.parameters)
if len(values) < len(ids):
return ()
else:
values = []
for motor_id in ids:
rp = self._protocol.DxlReadDataPacket(motor_id, control.address, control.length * control.nb_elem)
sp = self._send_packet(rp, error_handler=error_handler)
if not sp:
return ()
values.extend(sp.parameters)
values = list(zip(*([iter(values)] * control.length * control.nb_elem)))
values = [dxl_decode_all(value, control.nb_elem) for value in values]
if not values:
return ()
# when using SYNC_READ instead of getting a timeout
# a non existing motor will "return" the maximum value
if self._sync_read and self._protocol.name == 'v1':
max_val = 2 ** (8 * control.length) - 1
if max_val in (itertools.chain(*values) if control.nb_elem > 1 else values):
lost_ids = []
for i, v in enumerate(itertools.chain(*values) if control.nb_elem > 1 else values):
if v == max_val:
lost_ids.append(ids[i // control.nb_elem])
e = DxlTimeoutError(self, rp, list(set(lost_ids)))
if self._error_handler:
self._error_handler.handle_timeout(e)
return ()
else:
raise e
if convert:
models = self.get_model(ids)
if not models:
return ()
values = [control.dxl_to_si(v, m) for v, m in zip(values, models)]
return tuple(values)
def _set_control_value(self, control, value_for_id, **kwargs):
if not value_for_id:
return
convert = kwargs['convert'] if ('convert' in kwargs) else self._convert
if convert:
models = self.get_model(list(value_for_id.keys()))
if not models:
return
value_for_id = dict(zip(value_for_id.keys(),
map(control.si_to_dxl, value_for_id.values(), models)))
data = []
for motor_id, value in value_for_id.items():
data.extend(itertools.chain((motor_id, ),
dxl_code_all(value, control.length, control.nb_elem)))
wp = self._protocol.DxlSyncWritePacket(control.address, control.length * control.nb_elem, data)
self._send_packet(wp, wait_for_status_packet=False)
# MARK: - Send/Receive packet
def __real_send(self, instruction_packet, wait_for_status_packet, _force_lock):
if self.closed:
raise DxlError('try to send a packet on a closed serial communication')
logger.debug('Sending %s', instruction_packet,
extra={'port': self.port,
'baudrate': self.baudrate,
'timeout': self.timeout})
with self.__force_lock(_force_lock) or self._serial_lock:
self.flush(_force_lock=True)
data = instruction_packet.to_string()
try:
nbytes = self._serial.write(data)
except serial.serialutil.SerialTimeoutException:
nbytes = 0
if len(data) != nbytes:
raise DxlCommunicationError(self,
'instruction packet not entirely sent',
instruction_packet)
if not wait_for_status_packet:
return
status_packet = self.__real_read(instruction_packet, _force_lock=True)
logger.debug('Receiving %s', status_packet,
extra={'port': self.port,
'baudrate': self.baudrate,
'timeout': self.timeout})
return status_packet
def __real_read(self, instruction_packet, _force_lock):
with self.__force_lock(_force_lock) or self._serial_lock:
data = self._serial.read(self._protocol.DxlPacketHeader.length)
if not data:
raise DxlTimeoutError(self, instruction_packet, instruction_packet.id)
try:
header = self._protocol.DxlPacketHeader.from_string(data)
data += self._serial.read(header.packet_length)
status_packet = self._protocol.DxlStatusPacket.from_string(data)
except ValueError:
msg = 'could not parse received data {}'.format(bytearray(data))
raise DxlCommunicationError(self, msg, instruction_packet)
return status_packet
def _send_packet(self,
instruction_packet, wait_for_status_packet=True,
error_handler=None,
_force_lock=False):
if not error_handler:
return self.__real_send(instruction_packet, wait_for_status_packet, _force_lock)
try:
sp = self.__real_send(instruction_packet, wait_for_status_packet, _force_lock)
if sp and sp.error:
errors = decode_error(sp.error)
for e in errors:
handler_name = 'handle_{}'.format(e.lower().replace(' ', '_'))
f = operator.methodcaller(handler_name, instruction_packet)
f(error_handler)
return sp
except DxlTimeoutError as e:
error_handler.handle_timeout(e)
except DxlCommunicationError as e:
error_handler.handle_communication_error(e)
# MARK: - Dxl Errors
class DxlError(Exception):
""" Base class for all errors encountered using :class:`~pypot.dynamixel.io.DxlIO`. """
pass
class DxlCommunicationError(DxlError):
""" Base error for communication error encountered when using :class:`~pypot.dynamixel.io.DxlIO`. """
def __init__(self, dxl_io, message, instruction_packet):
self.dxl_io = dxl_io
self.message = message
self.instruction_packet = instruction_packet
def __str__(self):
return '{self.message} after sending {self.instruction_packet}'.format(self=self)
class DxlTimeoutError(DxlCommunicationError):
""" Timeout error encountered when using :class:`~pypot.dynamixel.io.DxlIO`. """
def __init__(self, dxl_io, instruction_packet, ids):
DxlCommunicationError.__init__(self, dxl_io, 'timeout occured', instruction_packet)
self.ids = ids
def __str__(self):
return 'motors {} did not respond after sending {}'.format(self.ids, self.instruction_packet)
@contextmanager
def with_True():
yield True

279
pypot-master/pypot-master/pypot/dynamixel/io/io.py Normal file
View File

@ -0,0 +1,279 @@
import itertools
from .abstract_io import (AbstractDxlIO, _DxlControl, DxlCommunicationError,
_DxlAccess, DxlTimeoutError)
from .. import conversion as conv
from ..protocol import v1 as v1
class DxlIO(AbstractDxlIO):
_protocol = v1
def factory_reset(self):
""" Reset all motors on the bus to their factory default settings. """
# Recent firmwares can't send the reset packet to broadcast, so the motor won't reply
for id in range(253):
try:
self._send_packet(self._protocol.DxlResetPacket(id))
except (DxlTimeoutError, DxlCommunicationError):
pass
else:
break
def get_control_mode(self, ids):
""" Gets the mode ('joint' or 'wheel') for the specified motors. """
to_get_ids = [id for id in ids if id not in self._known_mode]
limits = self.get_angle_limit(to_get_ids, convert=False)
modes = ['wheel' if limit == (0, 0) else 'joint' for limit in limits]
self._known_mode.update(zip(to_get_ids, modes))
return tuple(self._known_mode[id] for id in ids)
def set_wheel_mode(self, ids):
""" Sets the specified motors to wheel mode. """
self.set_control_mode(dict(zip(ids, itertools.repeat('wheel'))))
def set_joint_mode(self, ids):
""" Sets the specified motors to joint mode. """
self.set_control_mode(dict(zip(ids, itertools.repeat('joint'))))
def set_control_mode(self, mode_for_id):
models = []
for m in self.get_model(list(mode_for_id.keys())):
if m.startswith('MX'):
models += ['MX']
elif m.startswith('SR'):
models += ['SR']
elif m.startswith('EX'):
models += ['EX']
else:
models += ['*']
pos_max = [conv.position_range[m][0] for m in models]
limits = ((0, 0) if mode == 'wheel' else (0, pos_max[i] - 1)
for i, mode in enumerate(mode_for_id.values()))
self._set_angle_limit(dict(zip(mode_for_id.keys(), limits)), convert=False)
self._known_mode.update(mode_for_id.items())
def set_angle_limit(self, limit_for_id, **kwargs):
""" Sets the angle limit to the specified motors. """
convert = kwargs['convert'] if 'convert' in kwargs else self._convert
if 'wheel' in self.get_control_mode(list(limit_for_id.keys())):
raise ValueError('can not change the angle limit of a motor in wheel mode')
if (0, 0) in limit_for_id.values():
raise ValueError('can not set limit to (0, 0)')
self._set_angle_limit(limit_for_id, convert=convert)
# MARK: - Generate the accessors
def _add_control(name,
address, length=2, nb_elem=1,
access=_DxlAccess.readwrite,
models=set(conv.dynamixelModels.values()),
dxl_to_si=lambda val, model: val,
si_to_dxl=lambda val, model: val,
getter_name=None,
setter_name=None):
control = _DxlControl(name,
address, length, nb_elem,
access,
models,
dxl_to_si, si_to_dxl,
getter_name, setter_name)
DxlIO._generate_accessors(control)
_add_control('model',
address=0x00,
access=_DxlAccess.readonly,
dxl_to_si=conv.dxl_to_model)
_add_control('firmware',
address=0x02, length=1,
access=_DxlAccess.readonly)
_add_control('id',
address=0x03, length=1,
access=_DxlAccess.writeonly,
setter_name='change_id')
_add_control('baudrate',
address=0x04, length=1,
access=_DxlAccess.writeonly,
setter_name='change_baudrate',
si_to_dxl=conv.baudrate_to_dxl)
_add_control('return delay time',
address=0x05, length=1,
dxl_to_si=conv.dxl_to_rdt,
si_to_dxl=conv.rdt_to_dxl)
_add_control('angle limit',
address=0x06, nb_elem=2,
dxl_to_si=lambda value, model: (conv.dxl_to_degree(value[0], model),
conv.dxl_to_degree(value[1], model)),
si_to_dxl=lambda value, model: (conv.degree_to_dxl(value[0], model),
conv.degree_to_dxl(value[1], model)))
_add_control('drive mode',
address=0x0A, length=1,
access=_DxlAccess.readwrite,
models=('MX-106', ),
dxl_to_si=conv.dxl_to_drive_mode,
si_to_dxl=conv.drive_mode_to_dxl)
_add_control('highest temperature limit',
address=0x0B, length=1,
dxl_to_si=conv.dxl_to_temperature,
si_to_dxl=conv.temperature_to_dxl)
_add_control('voltage limit',
address=0x0C, length=1, nb_elem=2,
dxl_to_si=lambda value, model: (conv.dxl_to_voltage(value[0], model),
conv.dxl_to_voltage(value[1], model)),
si_to_dxl=lambda value, model: (conv.voltage_to_dxl(value[0], model),
conv.voltage_to_dxl(value[1], model)))
_add_control('max torque',
address=0x0E,
dxl_to_si=conv.dxl_to_torque,
si_to_dxl=conv.torque_to_dxl)
_add_control('status return level',
address=0x10, length=1,
dxl_to_si=conv.dxl_to_status,
si_to_dxl=conv.status_to_dxl)
_add_control('alarm LED',
address=0x11, length=1,
dxl_to_si=conv.dxl_to_alarm,
si_to_dxl=conv.alarm_to_dxl)
_add_control('alarm shutdown',
address=0x12, length=1,
dxl_to_si=conv.dxl_to_alarm,
si_to_dxl=conv.alarm_to_dxl)
_add_control('torque_enable',
address=0x18, length=1,
dxl_to_si=conv.dxl_to_bool,
si_to_dxl=conv.bool_to_dxl,
getter_name='is_torque_enabled',
setter_name='_set_torque_enable')
_add_control('LED',
address=0x19, length=1,
dxl_to_si=conv.dxl_to_bool,
si_to_dxl=conv.bool_to_dxl,
setter_name='_set_LED',
getter_name='is_led_on')
_add_control('pid gain',
address=0x1A, length=1, nb_elem=3,
models=('MX-12', 'MX-28', 'MX-64', 'MX-106'),
dxl_to_si=conv.dxl_to_pid,
si_to_dxl=conv.pid_to_dxl)
_add_control('compliance margin',
address=0x1A, length=1, nb_elem=2,
models=('AX-12', 'AX-18', 'RX-24', 'RX-28', 'RX-64'))
_add_control('compliance slope',
address=0x1C, length=1, nb_elem=2,
models=('AX-12', 'AX-18', 'RX-24', 'RX-28', 'RX-64'))
_add_control('goal position',
address=0x1E,
dxl_to_si=conv.dxl_to_degree,
si_to_dxl=conv.degree_to_dxl)
_add_control('moving speed',
address=0x20,
dxl_to_si=conv.dxl_to_speed,
si_to_dxl=conv.speed_to_dxl)
_add_control('torque limit',
address=0x22,
dxl_to_si=conv.dxl_to_torque,
si_to_dxl=conv.torque_to_dxl)
_add_control('goal position speed load',
address=0x1E, nb_elem=3,
dxl_to_si=lambda value, model: (conv.dxl_to_degree(value[0], model),
conv.dxl_to_speed(value[1], model),
conv.dxl_to_load(value[2], model)),
si_to_dxl=lambda value, model: (conv.degree_to_dxl(value[0], model),
conv.speed_to_dxl(value[1], model),
conv.torque_to_dxl(value[2], model)))
_add_control('present position',
address=0x24,
access=_DxlAccess.readonly,
dxl_to_si=conv.dxl_to_degree)
_add_control('present speed',
address=0x26,
access=_DxlAccess.readonly,
dxl_to_si=conv.dxl_to_speed)
_add_control('present load',
address=0x28,
access=_DxlAccess.readonly,
dxl_to_si=conv.dxl_to_load)
_add_control('present position speed load',
address=0x24, nb_elem=3,
access=_DxlAccess.readonly,
dxl_to_si=lambda value, model: (conv.dxl_to_degree(value[0], model),
conv.dxl_to_speed(value[1], model),
conv.dxl_to_load(value[2], model)))
_add_control('present voltage',
address=0x2A, length=1,
access=_DxlAccess.readonly,
dxl_to_si=conv.dxl_to_voltage)
_add_control('present temperature',
address=0x2B, length=1,
access=_DxlAccess.readonly,
dxl_to_si=conv.dxl_to_temperature)
_add_control('moving',
address=0x2E, length=1,
access=_DxlAccess.readonly,
dxl_to_si=conv.dxl_to_bool,
getter_name='is_moving')
_add_control('punch',
address=0x30, length=2,
models=('AX-12', 'AX-18', 'RX-24', 'RX-28', 'RX-64'))
_add_control('present current',
address=0x44,
access=_DxlAccess.readonly,
models=('MX-64', 'MX-106', 'SR-RH4D',),
dxl_to_si=conv.dxl_to_current)
_add_control('force control enable',
address=0x46, length=1,
models=('SR-RH4D',),
dxl_to_si=conv.dxl_to_bool,
si_to_dxl=conv.bool_to_dxl)
_add_control('goal force',
address=0x47,
models=('SR-RH4D',))
_add_control('goal acceleration',
address=0x49, length=1,
models=('MX-12, MX-28', 'MX-64', 'MX-106'),
dxl_to_si=conv.dxl_to_acceleration,
si_to_dxl=conv.acceleration_to_dxl)

248
pypot-master/pypot-master/pypot/dynamixel/io/io_320.py Normal file
View File

@ -0,0 +1,248 @@
from itertools import repeat
from .abstract_io import (AbstractDxlIO, _DxlControl, _DxlAccess,
DxlTimeoutError, DxlCommunicationError)
from .. import conversion as conv
from ..protocol import v2 as v2
class Dxl320IO(AbstractDxlIO):
_protocol = v2
def set_wheel_mode(self, ids):
self.set_control_mode(dict(zip(ids, repeat('wheel'))))
def set_joint_mode(self, ids):
self.set_control_mode(dict(zip(ids, repeat('joint'))))
def get_goal_position_speed_load(self, ids):
a = self._get_goal_pos_speed(ids)
b = self.get_torque_limit(ids)
return list(zip(*list(zip(*a)) + [b]))
def set_goal_position_speed_load(self, value_for_ids):
values = list(zip(*list(value_for_ids.values())))
self._set_goal_pos_speed(dict(zip(value_for_ids.keys(),
zip(*(values[0], values[1])))))
self.set_torque_limit(dict(zip(value_for_ids.keys(), values[2])))
def factory_reset(self, ids, except_ids=False, except_baudrate_and_ids=False):
""" Reset all motors on the bus to their factory default settings. """
mode = (0x02 if except_baudrate_and_ids else
0x01 if except_ids else 0xFF)
for id in ids:
try:
self._send_packet(self._protocol.DxlResetPacket(id, mode))
except (DxlTimeoutError, DxlCommunicationError):
pass
# TODO:
# * error
# MARK: - Generate the accessors
controls = {
# EEPROM
'model': {
'address': 0x00,
'access': _DxlAccess.readonly,
'dxl_to_si': conv.dxl_to_model
},
'firmware': {
'address': 0x02,
'length': 1,
'access': _DxlAccess.readonly
},
'id': {
'address': 0x03,
'length': 1,
'access': _DxlAccess.writeonly,
'setter_name': 'change_id'
},
'baudrate': {
'address': 0x04,
'length': 1,
'access': _DxlAccess.writeonly,
'setter_name': 'change_baudrate',
'si_to_dxl': conv.baudrate_to_dxl
},
'return delay time': {
'address': 0x05,
'length': 1,
'dxl_to_si': conv.dxl_to_rdt,
'si_to_dxl': conv.rdt_to_dxl
},
'angle limit': {
'address': 0x06,
'nb_elem': 2,
'dxl_to_si': lambda value, model: (conv.dxl_to_degree(value[0], model),
conv.dxl_to_degree(value[1], model)),
'si_to_dxl': lambda value, model: (conv.degree_to_dxl(value[0], model),
conv.degree_to_dxl(value[1], model))
},
'control mode': {
'address': 0x0B,
'length': 1,
'dxl_to_si': conv.dxl_to_control_mode,
'si_to_dxl': conv.control_mode_to_dxl,
},
'highest temperature limit': {
'address': 0x0C,
'length': 1,
'dxl_to_si': conv.dxl_to_temperature,
'si_to_dxl': conv.temperature_to_dxl
},
'voltage limit': {
'address': 0x0D,
'length': 1,
'nb_elem': 2,
'dxl_to_si': lambda value, model: (conv.dxl_to_voltage(value[0], model),
conv.dxl_to_voltage(value[1], model)),
'si_to_dxl': lambda value, model: (conv.voltage_to_dxl(value[0], model),
conv.voltage_to_dxl(value[1], model))
},
'max torque': {
'address': 0x0F,
'dxl_to_si': conv.dxl_to_torque,
'si_to_dxl': conv.torque_to_dxl
},
'status return level': {
'address': 0x11,
'length': 1,
'dxl_to_si': conv.dxl_to_status,
'si_to_dxl': conv.status_to_dxl
},
'alarm shutdown': {
'address': 0x12,
'length': 1,
'dxl_to_si': conv.dxl_to_alarm,
'si_to_dxl': conv.alarm_to_dxl
},
# RAM
'torque_enable': {
'address': 0x18,
'length': 1,
'dxl_to_si': conv.dxl_to_bool,
'si_to_dxl': conv.bool_to_dxl,
'getter_name': 'is_torque_enabled',
'setter_name': '_set_torque_enable'
},
'LED': {
'address': 0x19,
'length': 1,
'dxl_to_si': conv.dxl_to_bool,
'si_to_dxl': conv.bool_to_dxl,
'setter_name': '_set_LED',
'getter_name': 'is_led_on'
},
'LED color': {
'address': 0x19,
'length': 1,
'dxl_to_si': conv.dxl_to_led_color,
'si_to_dxl': conv.led_color_to_dxl,
},
'pid gain': {
'address': 0x1B,
'length': 1,
'nb_elem': 3,
'dxl_to_si': conv.dxl_to_pid,
'si_to_dxl': conv.pid_to_dxl
},
'goal position': {
'address': 0x1E,
'dxl_to_si': conv.dxl_to_degree,
'si_to_dxl': conv.degree_to_dxl
},
'moving speed': {
'address': 0x20,
'dxl_to_si': conv.dxl_to_speed,
'si_to_dxl': conv.speed_to_dxl
},
'torque limit': {
'address': 0x23,
'dxl_to_si': conv.dxl_to_torque,
'si_to_dxl': conv.torque_to_dxl
},
'goal position speed': {
'address': 0x1E,
'nb_elem': 2,
'dxl_to_si': lambda value, model: (conv.dxl_to_degree(value[0], model),
conv.dxl_to_speed(value[1], model)),
'si_to_dxl': lambda value, model: (conv.degree_to_dxl(value[0], model),
conv.speed_to_dxl(value[1], model)),
'getter_name': '_get_goal_pos_speed',
'setter_name': '_set_goal_pos_speed'
},
'present position': {
'address': 0x25,
'access': _DxlAccess.readonly,
'dxl_to_si': conv.dxl_to_degree
},
'present speed': {
'address': 0x27,
'access': _DxlAccess.readonly,
'dxl_to_si': conv.dxl_to_speed
},
'present load': {
'address': 0x29,
'access': _DxlAccess.readonly,
'dxl_to_si': conv.dxl_to_load
},
'present position speed load': {
'address': 0x25,
'nb_elem': 3,
'access': _DxlAccess.readonly,
'dxl_to_si': lambda value, model: (conv.dxl_to_degree(value[0], model),
conv.dxl_to_speed(value[1], model),
conv.dxl_to_load(value[2], model))
},
'present voltage': {
'address': 0x2D,
'length': 1,
'access': _DxlAccess.readonly,
'dxl_to_si': conv.dxl_to_voltage
},
'present temperature': {
'address': 0x2E,
'length': 1,
'access': _DxlAccess.readonly,
'dxl_to_si': conv.dxl_to_temperature
},
'moving': {
'address': 0x31,
'length': 1,
'access': _DxlAccess.readonly,
'dxl_to_si': conv.dxl_to_bool,
'getter_name': 'is_moving'
}
}
def _add_control(name,
address, length=2, nb_elem=1,
access=_DxlAccess.readwrite,
models=['XL-320', ],
dxl_to_si=lambda val, model: val,
si_to_dxl=lambda val, model: val,
getter_name=None,
setter_name=None):
control = _DxlControl(name,
address, length, nb_elem,
access,
models,
dxl_to_si, si_to_dxl,
getter_name, setter_name)
Dxl320IO._generate_accessors(control)
for name, args in controls.items():
_add_control(name, **args)

214
pypot-master/pypot-master/pypot/dynamixel/io/io_xl330.py Normal file
View File

@ -0,0 +1,214 @@
import pypot.dynamixel.conversion as conv
from pypot.dynamixel.io.abstract_io import AbstractDxlIO, _DxlAccess, _DxlControl
from pypot.dynamixel.protocol import v2 as v2
max_pos = 4096
max_deg = 360
max_current = 1750
def dxl_to_degree(value, model):
return round(((max_deg * float(value)) / (max_pos - 1)) - (max_deg / 2), 2)
def dxl_to_current(value, model):
if value > 0x7FFF:
value = value - 65536
# value = value >> 1
# print("{0:b}".format(value))
return value
def degree_to_dxl(value, model):
pos = int(round((max_pos - 1) * ((max_deg / 2 + float(value)) / max_deg), 0))
pos = min(max(pos, 0), max_pos - 1)
return pos
def dxl_to_velocity(value, model):
if value > 2 ** (4 * 8 - 1):
value = value - 2 ** (4 * 8)
return value
def baudrate_to_dxl(value, model):
current_baudrates = {0: 9600.0, 1: 57600.0, 2: 115200.0, 3: 1000000.0, 4: 2000000.0}
for k, v in current_baudrates.items():
if (abs(v - value) / float(value)) < 0.05:
return k
raise ValueError(
"incorrect baudrate {} (possible values {})".format(
value, list(current_baudrates.values())
)
)
class DxlXL330IO(AbstractDxlIO):
_protocol = v2
controls = {
# EEPROM
"model": {
"address": 0x00,
"access": _DxlAccess.readonly,
"dxl_to_si": conv.dxl_to_model,
},
"id": {
"address": 0x07,
"length": 1,
"access": _DxlAccess.writeonly,
"setter_name": "change_id",
},
"baudrate": {
"address": 0x08,
"length": 1,
"access": _DxlAccess.writeonly,
"setter_name": "change_baudrate",
"si_to_dxl": baudrate_to_dxl,
},
"return delay time": {
"address": 0x09,
"length": 1,
"dxl_to_si": conv.dxl_to_rdt,
"si_to_dxl": conv.rdt_to_dxl,
},
"control mode": {
"address": 0x10,
"length": 1,
},
"operating mode": {
"address": 0xB,
"length": 1,
},
"highest temperature limit": {
"address": 0x1F,
"length": 1,
"dxl_to_si": conv.dxl_to_temperature,
"si_to_dxl": conv.temperature_to_dxl,
},
"voltage limit": {
"address": 0x20,
"length": 1,
"nb_elem": 2,
"dxl_to_si": lambda value, model: (
conv.dxl_to_voltage(value[0], model),
conv.dxl_to_voltage(value[1], model),
),
"si_to_dxl": lambda value, model: (
conv.voltage_to_dxl(value[0], model),
conv.voltage_to_dxl(value[1], model),
),
},
"current limit": {
"address": 0x26,
"length": 2,
},
"angle limit": {
"address": 0x30,
"nb_elem": 2,
"length": 4,
"dxl_to_si": lambda value, model: (
dxl_to_degree(value[0], model),
dxl_to_degree(value[1], model),
),
"si_to_dxl": lambda value, model: (
degree_to_dxl(value[0], model),
degree_to_dxl(value[1], model),
),
},
"pwm slope": {
"address": 0x3E,
"length": 1,
},
# RAM
"torque_enable": {
"address": 0x40,
"length": 1,
"dxl_to_si": conv.dxl_to_bool,
"si_to_dxl": conv.bool_to_dxl,
"getter_name": "is_torque_enabled",
"setter_name": "_set_torque_enable",
},
"LED": {
"address": 0x41,
"length": 1,
"dxl_to_si": conv.dxl_to_bool,
"si_to_dxl": conv.bool_to_dxl,
"setter_name": "_set_LED",
"getter_name": "is_led_on",
},
"pid gain": {
"address": 0x50,
"length": 2,
"nb_elem": 3,
# "dxl_to_si": conv.dxl_to_pid,
# "si_to_dxl": conv.pid_to_dxl,
},
"goal position": {
"address": 0x74,
"length": 4,
"dxl_to_si": dxl_to_degree,
"si_to_dxl": degree_to_dxl,
},
"present velocity": {
"address": 0x80,
"length": 4,
"access": _DxlAccess.readonly,
"dxl_to_si": dxl_to_velocity,
},
"present position": {
"address": 0x84,
"length": 4,
"access": _DxlAccess.readonly,
"dxl_to_si": dxl_to_degree,
},
"present current": {
"address": 0x7E,
"length": 2,
"access": _DxlAccess.readonly,
"dxl_to_si": dxl_to_current,
},
"goal current": {
"address": 0x66,
"length": 2,
"access": _DxlAccess.readonly,
"dxl_to_si": dxl_to_current,
},
}
def _add_control(
name,
address,
length=2,
nb_elem=1,
access=_DxlAccess.readwrite,
models=[
"XL-330",
],
dxl_to_si=lambda val, model: val,
si_to_dxl=lambda val, model: val,
getter_name=None,
setter_name=None,
):
control = _DxlControl(
name,
address,
length,
nb_elem,
access,
models,
dxl_to_si,
si_to_dxl,
getter_name,
setter_name,
)
DxlXL330IO._generate_accessors(control)
for name, args in controls.items():
_add_control(name, **args)

188
pypot-master/pypot-master/pypot/dynamixel/io/io_xm430.py Normal file
View File

@ -0,0 +1,188 @@
from pypot.dynamixel.io.abstract_io import AbstractDxlIO, _DxlAccess, _DxlControl
from pypot.dynamixel.protocol import v2 as v2
import pypot.dynamixel.conversion as conv
class DxlXM430IO(AbstractDxlIO):
_protocol = v2
def baudrate_to_dxl(value, model):
current_baudrates = {0: 9600.0, 1: 57600.0, 2: 115200.0, 3: 1000000.0, 4: 2000000.0}
for k, v in current_baudrates.items():
if (abs(v - value) / float(value)) < 0.05:
return k
raise ValueError(
"incorrect baudrate {} (possible values {})".format(
value, list(current_baudrates.values())
)
)
controls = {
# EEPROM
"model": {
"address": 0x00,
"access": _DxlAccess.readonly,
"dxl_to_si": conv.dxl_to_model,
},
"id": {
"address": 0x07,
"length": 1,
"access": _DxlAccess.writeonly,
"setter_name": "change_id",
},
"baudrate": {
"address": 0x08,
"length": 1,
"access": _DxlAccess.writeonly,
"setter_name": "change_baudrate",
"si_to_dxl": baudrate_to_dxl,
},
"return delay time": {
"address": 0x09,
"length": 1,
"dxl_to_si": conv.dxl_to_rdt,
"si_to_dxl": conv.rdt_to_dxl,
},
"drive mode": {
"address": 10,
"length": 1,
},
"operating mode": {
"address": 11,
"length": 1,
},
"highest temperature limit": {
"address": 31,
"length": 1,
"dxl_to_si": conv.dxl_to_temperature,
"si_to_dxl": conv.temperature_to_dxl,
},
"max voltage limit": {
"address": 32,
"length": 2,
"dxl_to_si": conv.dxl_to_voltage,
"si_to_dxl": conv.voltage_to_dxl,
},
"min voltage limit": {
"address": 34,
"length": 2,
"dxl_to_si": conv.dxl_to_voltage,
"si_to_dxl": conv.voltage_to_dxl,
},
"current limit": {
"address": 38,
"length": 2,
},
"max angle limit": {
"address": 48,
"length": 4,
"dxl_to_si": conv.dxl_to_degree,
"si_to_dxl": conv.degree_to_dxl,
},
"min angle limit": {
"address": 52,
"length": 4,
"dxl_to_si": conv.dxl_to_degree,
"si_to_dxl": conv.degree_to_dxl,
},
"pwm slope": {
"address": 0x3E,
"length": 1,
},
# RAM
"torque_enable": {
"address": 64,
"length": 1,
"dxl_to_si": conv.dxl_to_bool,
"si_to_dxl": conv.bool_to_dxl,
"getter_name": "is_torque_enabled",
"setter_name": "_set_torque_enable",
},
"LED": {
"address": 65,
"length": 1,
"dxl_to_si": conv.dxl_to_bool,
"si_to_dxl": conv.bool_to_dxl,
"setter_name": "_set_LED",
"getter_name": "is_led_on",
},
"dip gain": {
"address": 80,
"length": 2,
"nb_elem": 3,
# "dxl_to_si": conv.dxl_to_pid,
# "si_to_dxl": conv.pid_to_dxl,
},
"goal position": {
"address": 116,
"length": 4,
"dxl_to_si": conv.dxl_to_degree,
"si_to_dxl": conv.degree_to_dxl,
},
"present velocity": {
"address": 128,
"length": 4,
"access": _DxlAccess.readonly,
},
"present position": {
"address": 132,
"length": 4,
"access": _DxlAccess.readonly,
"dxl_to_si": conv.dxl_to_degree,
},
"present current": {
"address": 126,
"length": 2,
"access": _DxlAccess.readonly,
},
"goal current": {
"address": 102,
"length": 2,
"access": _DxlAccess.readonly,
},
"present temperature": {
"address": 146,
"length": 1,
"access": _DxlAccess.readonly,
},
}
def _add_control(
name,
address,
length=2,
nb_elem=1,
access=_DxlAccess.readwrite,
models=[
"XM-430",
],
dxl_to_si=lambda val, model: val,
si_to_dxl=lambda val, model: val,
getter_name=None,
setter_name=None,
):
control = _DxlControl(
name,
address,
length,
nb_elem,
access,
models,
dxl_to_si,
si_to_dxl,
getter_name,
setter_name,
)
DxlXM430IO._generate_accessors(control)
for name, args in controls.items():
_add_control(name, **args)

407
pypot-master/pypot-master/pypot/dynamixel/motor.py Normal file
View File

@ -0,0 +1,407 @@
import numpy
import logging
from collections import defaultdict
import pypot.utils.pypot_time as time
from ..robot.motor import Motor
from ..utils import SyncEvent
from ..utils.trajectory import GotoMinJerk, GotoLinear
from ..utils.stoppablethread import StoppableLoopThread
logger = logging.getLogger(__name__)
class DxlRegister(object):
def __init__(self, rw=False):
self.rw = rw
def __get__(self, instance, owner):
if instance._read_synchronous[self.label]:
sync = instance._read_synced[self.label]
if not sync.is_recent:
sync.request()
value = instance.__dict__.get(self.label, 0)
return value
def __set__(self, instance, value):
if not self.rw:
raise AttributeError("can't set attribute")
logger.debug("Setting '%s.%s' to %s",
instance.name, self.label, value)
instance.__dict__[self.label] = value
if instance._write_synchronous[self.label]:
sync = instance._write_synced[self.label]
sync.request()
class DxlOrientedRegister(DxlRegister):
def __get__(self, instance, owner):
value = DxlRegister.__get__(self, instance, owner)
return (value if instance.direct else -value)
def __set__(self, instance, value):
value = value if instance.direct else -value
DxlRegister.__set__(self, instance, value)
class DxlPositionRegister(DxlOrientedRegister):
def __get__(self, instance, owner):
value = DxlOrientedRegister.__get__(self, instance, owner)
return value - instance.offset
def __set__(self, instance, value):
value = value + instance.offset
DxlOrientedRegister.__set__(self, instance, value)
class RegisterOwner(type):
def __new__(cls, name, bases, attrs):
for n, v in attrs.items():
if isinstance(v, DxlRegister):
v.label = n
attrs['registers'].append(n)
return super(RegisterOwner, cls).__new__(cls, name, bases, attrs)
class DxlMotor(Motor, metaclass=RegisterOwner):
""" High-level class used to represent and control a generic dynamixel motor.
This class provides all level access to (see :attr:`~pypot.dynamixel.motor.DxlMotor.registers` for an exhaustive list):
* motor id
* motor name
* motor model
* present position/speed/load
* goal position/speed/load
* compliant
* motor orientation and offset
* angle limit
* temperature
* voltage
This class represents a generic robotis motor and you define your own subclass for specific motors (see :class:`~pypot.dynamixel.motor.DxlMXMotor` or :class:`~pypot.dynamixel.motor.DxlAXRXMotor`).
Those properties are synchronized with the real motors values thanks to a :class:`~pypot.dynamixel.controller.DxlController`.
"""
registers = Motor.registers + ['registers',
'goal_speed',
'compliant', 'safe_compliant',
'angle_limit']
id = DxlRegister()
name = DxlRegister()
model = DxlRegister()
present_position = DxlPositionRegister()
goal_position = DxlPositionRegister(rw=True)
present_speed = DxlOrientedRegister()
moving_speed = DxlOrientedRegister(rw=True)
present_load = DxlOrientedRegister()
torque_limit = DxlRegister(rw=True)
lower_limit = DxlPositionRegister()
upper_limit = DxlPositionRegister()
present_voltage = DxlRegister()
present_temperature = DxlRegister()
def __init__(self, id, name=None, model='',
direct=True, offset=0.0,
broken=False,
angle_limit=None):
self.__dict__['id'] = id
name = name if name is not None else 'motor_{}'.format(id)
self.__dict__['name'] = name
self.__dict__['model'] = model
self.__dict__['direct'] = direct
self.__dict__['offset'] = offset
self.__dict__['compliant'] = True
self._safe_compliance = SafeCompliance(self)
self.goto_behavior = 'dummy'
self.compliant_behavior = 'dummy'
self._broken = broken
self._read_synchronous = defaultdict(lambda: False)
self._read_synced = defaultdict(SyncEvent)
self._write_synchronous = defaultdict(lambda: False)
self._write_synced = defaultdict(SyncEvent)
if angle_limit is not None:
self.__dict__['lower_limit'], self.__dict__['upper_limit'] = angle_limit
def __repr__(self):
return ('<DxlMotor name={self.name} '
'id={self.id} '
'pos={self.present_position}>').format(self=self)
@property
def goal_speed(self):
""" Goal speed (in degrees per second) of the motor.
This property can be used to control your motor in speed. Setting a goal speed will automatically change the moving speed and sets the goal position as the angle limit.
.. note:: The motor will turn until reaching the angle limit. But this is not a wheel mode, so the motor will stop at its limits.
"""
return numpy.sign(self.goal_position) * self.moving_speed
@goal_speed.setter
def goal_speed(self, value):
if abs(value) < numpy.finfo(numpy.float).eps:
self.goal_position = self.present_position
else:
# 0.7 corresponds approx. to the min speed that will be converted into 0
# and as 0 corresponds to setting the max speed, we have to check this case
value = numpy.sign(value) * 0.7 if abs(value) < 0.7 else value
self.goal_position = numpy.sign(value) * self.max_pos
self.moving_speed = abs(value)
@property
def compliant_behavior(self):
return self._compliant_behavior
@compliant_behavior.setter
def compliant_behavior(self, value):
if value not in ('dummy', 'safe'):
raise ValueError('Wrong compliant type! It should be either "dummy" or "safe".')
if hasattr(self, '_compliant_behavior') and self._compliant_behavior == value:
return
self._compliant_behavior = value
# Start the safe compliance behavior when the motor should be compliant
if value == 'safe' and self.compliant:
self._safe_compliance.start()
if value == 'dummy':
use_safe = self._safe_compliance.started
if use_safe:
self._safe_compliance.stop()
self.compliant = self.compliant or use_safe
@property
def compliant(self):
return bool(self.__dict__['compliant'])
@compliant.setter
def compliant(self, is_compliant):
if self._safe_compliance.started and is_compliant:
return
if self.compliant_behavior == 'dummy':
self._set_compliancy(is_compliant)
elif self.compliant_behavior == 'safe':
if is_compliant:
self._safe_compliance.start()
elif self._safe_compliance.started:
self._safe_compliance.stop()
def _set_compliancy(self, is_compliant):
# Change the goal_position only if you switch from compliant to not compliant mode
if not is_compliant and self.compliant:
self.goal_position = self.present_position
self.__dict__['compliant'] = is_compliant
@property
def angle_limit(self):
return self.lower_limit, self.upper_limit
@angle_limit.setter
def angle_limit(self, limits):
self.lower_limit, self.upper_limit = limits
@property
def goto_behavior(self):
return self._default_goto_behavior
@goto_behavior.setter
def goto_behavior(self, value):
if value not in ('dummy', 'minjerk', 'linear'):
raise ValueError('Wrong compliant type! It should be either "dummy", "minjerk" or "linear".')
self._default_goto_behavior = value
def goto_position(self, position, duration, control=None, wait=False):
""" Automatically sets the goal position and the moving speed to reach the desired position within the duration. """
if control is None:
control = self.goto_behavior
if control == 'minjerk':
goto_min_jerk = GotoMinJerk(self, position, duration)
goto_min_jerk.start()
if wait:
goto_min_jerk.wait_to_stop()
elif control == 'dummy':
dp = abs(self.present_position - position)
speed = (dp / float(duration)) if duration > 0 else 0
self.moving_speed = speed
self.goal_position = position
if wait:
time.sleep(duration)
elif control == 'linear':
goto_linear = GotoLinear(self, position, duration)
goto_linear.start()
if wait:
goto_linear.wait_to_stop()
class DxlAXRXMotor(DxlMotor):
""" This class represents the AX robotis motor.
This class adds access to:
* compliance margin/slope (see the robotis website for details)
"""
registers = list(DxlMotor.registers)
compliance_margin = DxlRegister(rw=True)
compliance_slope = DxlRegister(rw=True)
def __init__(self, id, name=None, model='',
direct=True, offset=0.0, broken=False,
angle_limit=None):
DxlMotor.__init__(self, id, name, model,
direct, offset, broken,
angle_limit)
self.max_pos = 150
class DxlMXMotor(DxlMotor):
""" This class represents the RX and MX robotis motor.
This class adds access to:
* PID gains (see the robotis website for details)
"""
registers = list(DxlMotor.registers)
pid = DxlRegister(rw=True)
def __init__(self, id, name=None, model='',
direct=True, offset=0.0, broken=False,
angle_limit=None):
""" This class represents the RX and MX robotis motor.
This class adds access to:
* PID gains (see the robotis website for details)
"""
DxlMotor.__init__(self, id, name, model,
direct, offset, broken,
angle_limit)
self.max_pos = 180
class DxlMX64106Motor(DxlMXMotor):
""" This class represents the MX-64 and MX-106 robotis motor.
This class adds access to:
* present current
"""
registers = list(DxlMXMotor.registers)
present_current = DxlRegister()
def __init__(self, id, name=None, model='',
direct=True, offset=0.0, broken=False,
angle_limit=None):
""" This class represents the RX and MX robotis motor.
This class adds access to:
* PID gains (see the robotis website for details)
"""
DxlMotor.__init__(self, id, name, model,
direct, offset, broken,
angle_limit)
self.max_pos = 180
class DxlXL320Motor(DxlMXMotor):
registers = list(DxlMXMotor.registers)
led = DxlRegister(rw=True)
control_mode = DxlRegister(rw=True)
""" This class represents the XL-320 robotis motor. """
def __init__(self, id, name=None, model='XL-320',
direct=True, offset=0.0, broken=False,
angle_limit=None):
DxlMXMotor.__init__(self, id, name, model,
direct, offset, broken,
angle_limit)
self.max_pos = 150
class DxlSRMotor(DxlMotor):
""" This class represents the robotis motor found in the seed robotics hand.
This class adds access to:
* force control enable
* goal force
* present current
"""
registers = list(DxlMotor.registers)
force_control_enable = DxlRegister(rw=True)
goal_force = DxlRegister(rw=True)
present_current = DxlRegister()
def __init__(self, id, name=None, model='',
direct=True, offset=0.0, broken=False,
angle_limit=None):
""" This class represents the robotis motor found in the seed robotics hand.
This class adds access to:
* PID gains (see the robotis website for details)
* force control enable
* goal force
"""
DxlMotor.__init__(self, id, name, model,
direct, offset, broken,
angle_limit)
self.max_pos = 180
class SafeCompliance(StoppableLoopThread):
""" This class creates a controller to active compliance only if the current motor position is included in the angle limit, else the compliance is turned off. """
def __init__(self, motor, frequency=50):
StoppableLoopThread.__init__(self, frequency)
self.motor = motor
def update(self):
self.motor._set_compliancy((min(self.motor.angle_limit) < self.motor.present_position < max(self.motor.angle_limit)))
def teardown(self):
self.motor._set_compliancy(False)

0
pypot-master/pypot-master/pypot/dynamixel/protocol/__init__.py Normal file
View File

183
pypot-master/pypot-master/pypot/dynamixel/protocol/v1.py Normal file
View File

@ -0,0 +1,183 @@
# -*- coding: utf-8 -*-
import numpy
import itertools
from collections import namedtuple
name = 'v1'
DxlBroadcast = 254
class DxlInstruction(object):
PING = 0x01
READ_DATA = 0x02
WRITE_DATA = 0x03
RESET = 0x06
SYNC_WRITE = 0x83
SYNC_READ = 0x84
# MARK: - Packet Header
class DxlPacketHeader(namedtuple('DxlPacketHeader', ('id', 'packet_length'))):
""" This class represents the header of a Dxl Packet.
They are constructed as follows [0xFF, 0xFF, ID, LENGTH] where:
* ID represents the ID of the motor who received (resp. sent) the intruction (resp. status) packet.
* LENGTH represents the length of the rest of the packet
"""
length = 4
marker = bytearray((0xFF, 0xFF))
@classmethod
def from_string(cls, data):
header = bytearray(data)
if len(header) != cls.length or header[:len(cls.marker)] != cls.marker:
raise ValueError('try to parse corrupted data ({})'.format(header))
return cls(header[2], header[3])
# MARK: - Instruction Packet
class DxlInstructionPacket(namedtuple('DxlInstructionPacket',
('id', 'instruction', 'parameters'))):
""" This class is used to represent a dynamixel instruction packet.
An instruction packet is constructed as follows:
[0xFF, 0xFF, ID, LENGTH, INSTRUCTION, PARAM 1, PARAM 2, ..., PARAM N, CHECKSUM]
(for more details see http://support.robotis.com/en/product/dxl_main.htm)
"""
def to_array(self):
return bytearray(itertools.chain(DxlPacketHeader.marker,
(self.id, self.length, self.instruction),
self.parameters,
(self.checksum, )))
def to_string(self):
return bytes(self.to_array())
@property
def length(self):
return len(self.parameters) + 2
@property
def checksum(self):
return int(255 - ((self.id + self.length + self.instruction +
sum(self.parameters)) % 256))
class DxlPingPacket(DxlInstructionPacket):
""" This class is used to represent ping packet. """
def __new__(cls, id):
return DxlInstructionPacket.__new__(cls, id, DxlInstruction.PING, ())
def __repr__(self):
return 'DxlPingPacket(id={})'.format(self.id)
class DxlResetPacket(DxlInstructionPacket):
""" This class is used to represent reset packet. """
def __new__(cls, id=DxlBroadcast):
return DxlInstructionPacket.__new__(cls, id,
DxlInstruction.RESET, ())
class DxlReadDataPacket(DxlInstructionPacket):
""" This class is used to represent read data packet (to read value). """
def __new__(cls, id, address, length):
return DxlInstructionPacket.__new__(cls, id,
DxlInstruction.READ_DATA,
(address, length))
def __repr__(self):
return ('DxlReadDataPacket(id={}, address={}'
', length={})'.format(self.id,
self.parameters[0],
self.parameters[1]))
class DxlSyncReadPacket(DxlInstructionPacket):
""" This class is used to represent sync read packet (to synchronously read values). """
def __new__(cls, ids, address, length):
return DxlInstructionPacket.__new__(cls, DxlBroadcast,
DxlInstruction.SYNC_READ,
tuple(itertools.chain((address, length),
ids)))
def __repr__(self):
return ('DxlSyncReadDataPacket(ids={}, '
'address={}, length={})'.format(self.parameters[2:],
self.parameters[0],
self.parameters[1]))
class DxlWriteDataPacket(DxlInstructionPacket):
""" This class is used to represent write data packet (to write value). """
def __new__(cls, id, address, coded_value):
return DxlInstructionPacket.__new__(cls, id,
DxlInstruction.WRITE_DATA,
tuple(itertools.chain((address,),
coded_value)))
def __repr__(self):
return ('DxlWriteDataPacket(id={}, '
'address={}, value={})'.format(self.id,
self.parameters[0],
self.parameters[1:]))
class DxlSyncWritePacket(DxlInstructionPacket):
""" This class is used to represent sync write packet (to synchronously write values). """
def __new__(cls, address, length, id_value_couples):
return DxlInstructionPacket.__new__(cls, DxlBroadcast,
DxlInstruction.SYNC_WRITE,
tuple(itertools.chain((address, length),
id_value_couples)))
def __repr__(self):
address = self.parameters[0]
length = self.parameters[1]
a = numpy.array(self.parameters[2:]).reshape((-1, length + 1))
ids = a[:, 0]
values = [tuple(v) for v in a[:, 1:]]
return ('DxlSyncWriteDataPacket(ids={}, '
'address={}, length={}, values={})'.format(ids,
address,
length,
values))
# MARK: - Status Packet
class DxlStatusPacket(namedtuple('DxlStatusPacket', ('id', 'error', 'parameters'))):
""" This class is used to represent a dynamixel status packet.
A status packet is constructed as follows:
[0xFF, 0xFF, ID, LENGTH, ERROR, PARAM 1, PARAM 2, ..., PARAM N, CHECKSUM]
(for more details see http://support.robotis.com/en/product/dxl_main.htm)
"""
@classmethod
def from_string(cls, data):
packet = bytearray(data)
header = DxlPacketHeader.from_string(packet[:4])
if len(packet) != DxlPacketHeader.length + header.packet_length \
or cls._checksum(packet) != packet[-1]:
raise ValueError('try to parse corrupted data ({})'.format(packet))
return cls(header.id, packet[4], tuple(packet[5:-1]))
@classmethod
def _checksum(cls, packet):
return int(255 - (sum(packet[2:-1]) % 256))

233
pypot-master/pypot-master/pypot/dynamixel/protocol/v2.py Normal file
View File

@ -0,0 +1,233 @@
import numpy
import itertools
from collections import namedtuple
from ..conversion import dxl_code, dxl_decode
name = 'v2'
DxlBroadcast = 254
class DxlInstruction(object):
PING = 0x01
READ_DATA = 0x02
WRITE_DATA = 0x03
RESET = 0x06
SYNC_READ = 0x82
SYNC_WRITE = 0x83
# MARK: - Packet Header
class DxlPacketHeader(namedtuple('DxlPacketHeader', ('id', 'packet_length'))):
""" This class represents the header of a Dxl Packet.
They are constructed as follows [0xFF, 0xFF, 0xFD, 0x00, ID, LEN_L, LEN_H] where:
* ID represents the ID of the motor who received (resp. sent) the intruction (resp. status) packet.
* LEN_L, LEN_H represents the length of the rest of the packet
"""
length = 7
marker = bytearray((0xFF, 0xFF, 0xFD, 0x00))
@classmethod
def from_string(cls, data):
header = bytearray(data)
if len(header) != cls.length or header[:len(cls.marker)] != cls.marker:
raise ValueError('try to parse corrupted data ({})'.format(header))
return cls(header[4], dxl_decode((header[5], header[6])))
# MARK: - Instruction Packet
class DxlInstructionPacket(namedtuple('DxlInstructionPacket',
('id', 'instruction', 'parameters'))):
""" This class is used to represent a dynamixel instruction packet.
An instruction packet is constructed as follows:
[0xFF, 0xFF, 0xFD, 0x00, ID, LEN_L, LEN_H, INST, PARAM 1, PARAM 2, ..., PARAM N, CRC_L, CRC_H]
(for more details see http://support.robotis.com/en/product/dxl_main.htm)
"""
def _buff(self):
return bytearray(itertools.chain(DxlPacketHeader.marker,
(self.id, ),
dxl_code(self.length, 2),
(self.instruction, ),
self.parameters))
def to_array(self):
return self._buff() + bytearray(dxl_code(self.checksum, 2))
def to_string(self):
return bytes(self.to_array())
@property
def length(self):
return len(self.parameters) + 3
@property
def checksum(self):
return crc16(self._buff(), 5 + self.length)
class DxlPingPacket(DxlInstructionPacket):
""" This class is used to represent ping packet. """
def __new__(cls, id):
return DxlInstructionPacket.__new__(cls, id, DxlInstruction.PING, ())
def __repr__(self):
return 'DxlPingPacket(id={})'.format(self.id)
class DxlResetPacket(DxlInstructionPacket):
""" This class is used to represent factory reset packet. """
def __new__(cls, id, mode):
return DxlInstructionPacket.__new__(cls, id,
DxlInstruction.RESET, (mode, ))
class DxlReadDataPacket(DxlInstructionPacket):
""" This class is used to represent read data packet (to read value). """
def __new__(cls, id, address, length):
return DxlInstructionPacket.__new__(cls, id,
DxlInstruction.READ_DATA,
list(dxl_code(address, 2)) +
list(dxl_code(length, 2)))
def __repr__(self):
return 'DxlReadDataPacket(id={}, address={}, length={})'.format(
self.id,
dxl_decode(list(reversed(self.parameters[0:2]))),
dxl_decode(list(reversed(self.parameters[2:4]))))
class DxlSyncReadPacket(DxlInstructionPacket):
""" This class is used to represent sync read packet (to synchronously read values). """
def __new__(cls, ids, address, length):
return DxlInstructionPacket.__new__(cls, DxlBroadcast,
DxlInstruction.SYNC_READ,
list(dxl_code(address, 2)) +
list(dxl_code(length, 2)) +
list(ids))
def __repr__(self):
return ('DxlSyncReadDataPacket(ids={}, '
'address={}, length={})'.format(self.parameters[4:],
dxl_decode(self.parameters[0:2]),
dxl_decode(self.parameters[2:4])))
class DxlWriteDataPacket(DxlInstructionPacket):
""" This class is used to represent write data packet (to write value). """
def __new__(cls, id, address, coded_value):
return DxlInstructionPacket.__new__(cls, id,
DxlInstruction.WRITE_DATA,
list(dxl_code(address, 2)) +
list(coded_value))
def __repr__(self):
return ('DxlWriteDataPacket(id={}, address={}, value={})'.format(
self.id,
dxl_decode(self.parameters[0:2]),
tuple(self.parameters[2:])))
class DxlSyncWritePacket(DxlInstructionPacket):
""" This class is used to represent sync write packet (to synchronously write values). """
def __new__(cls, address, length, id_value_couples):
return DxlInstructionPacket.__new__(cls, DxlBroadcast,
DxlInstruction.SYNC_WRITE,
list(itertools.chain(dxl_code(address, 2),
dxl_code(length, 2),
id_value_couples)))
def __repr__(self):
address = dxl_decode(self.parameters[0:2])
length = dxl_decode(self.parameters[2:4])
a = numpy.array(self.parameters[4:]).reshape((-1, length + 1))
ids = a[:, 0]
values = [tuple(v) for v in a[:, 1:]]
return ('DxlSyncWriteDataPacket(ids={}, '
'address={}, length={}, values={})'.format(ids,
address,
length,
values))
# MARK: - Status Packet
class DxlStatusPacket(namedtuple('DxlStatusPacket', ('id', 'error', 'parameters'))):
""" This class is used to represent a dynamixel status packet.
A status packet is constructed as follows:
[0xFF, 0xFF, 0xFD, 0x00, ID, LEN_L, LEN_H, 0x55, ERROR, PARAM 1, PARAM 2, ..., PARAM N, CRC_L, CRC_H]
(for more details see http://support.robotis.com/en/product/dxl_main.htm)
"""
@classmethod
def from_string(cls, data):
packet = bytearray(data)
header = DxlPacketHeader.from_string(packet[:DxlPacketHeader.length])
if (len(packet) != DxlPacketHeader.length + header.packet_length or
cls._checksum(packet) != packet[-2:]):
raise ValueError('try to parse corrupted data ({})'.format(packet))
return cls(header.id, packet[8], tuple(packet[9:-2]))
@classmethod
def _checksum(cls, packet):
return bytearray(dxl_code(crc16(packet[:-2], len(packet) - 2), 2))
def crc16(data_blk, data_blk_size, crc_accum=0):
for j in range(data_blk_size):
i = ((crc_accum >> 8) ^ data_blk[j]) & 0xFF
crc_accum = ((crc_accum << 8) ^ crc_table[i]) % (2 ** 16)
return crc_accum
crc_table = [
0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
]

119
pypot-master/pypot-master/pypot/dynamixel/syncloop.py Normal file
View File

@ -0,0 +1,119 @@
from ..robot.controller import MotorsController
from .controller import (DxlController,
PosSpeedLoadDxlController,
AngleLimitRegisterController)
class MetaDxlController(MotorsController):
""" Synchronizes the reading/writing of :class:`~pypot.dynamixel.motor.DxlMotor` with the real motors.
This class handles synchronization loops that automatically read/write values from the "software" :class:`~pypot.dynamixel.motor.DxlMotor` with their "hardware" equivalent. Those loops shared a same :class:`~pypot.dynamixel.io.DxlIO` connection to avoid collision in the bus. Each loop run within its own thread as its own frequency.
.. warning:: As all the loop attached to a controller shared the same bus, you should make sure that they can run without slowing down the other ones.
"""
def __init__(self, io, motors, controllers):
MotorsController.__init__(self, io, motors, 1.)
self.controllers = controllers
def setup(self):
""" Starts all the synchronization loops. """
[c.start() for c in self.controllers]
[c.wait_to_start() for c in self.controllers]
def update(self):
pass
def teardown(self):
""" Stops the synchronization loops. """
[c.stop() for c in self.controllers]
class BaseDxlController(MetaDxlController):
""" Implements a basic controller that synchronized the most frequently used values.
More precisely, this controller:
* reads the present position, speed, load at 50Hz
* writes the goal position, moving speed and torque limit at 50Hz
* writes the pid gains (or compliance margin and slope) at 10Hz
* reads the present voltage and temperature at 1Hz
"""
def __init__(self, io, motors):
controllers = [
PosSpeedLoadDxlController(io, motors, 50.),
AngleLimitRegisterController(io, motors, 10., False),
DxlController(io, motors, 1., False, 'get', 'present_voltage'),
DxlController(io, motors, 1., False, 'get', 'present_temperature')
]
pid_motors = [m for m in motors
if (m.model.startswith('MX') or \
m.model.startswith('XL-320'))]
if pid_motors:
controllers.insert(0, DxlController(io, pid_motors, 10., False,
'set', 'pid_gain', 'pid'))
force_control_motors = [m for m in motors if m.model.startswith('SR')]
if force_control_motors:
controllers.insert(0, DxlController(io, force_control_motors, 10., False,
'set', 'force_control_enable', 'force_control_enable'))
controllers.insert(0, DxlController(io, force_control_motors, 10., False,
'set', 'goal_force', 'goal_force'))
current_motors = [m for m in motors
if (m.model.startswith('MX-64') or \
m.model.startswith('MX-106') or \
m.model.startswith('SR'))]
if current_motors:
controllers.insert(0, DxlController(io, current_motors, 10., False,
'get', 'present_current', 'present_current'))
margin_slope_motors = [m for m in motors
if (m.model.startswith('AX') or
m.model.startswith('RX'))]
if margin_slope_motors:
controllers.append(DxlController(io, margin_slope_motors, 10, False,
'set', 'compliance_margin'))
controllers.append(DxlController(io, margin_slope_motors, 10, False,
'set', 'compliance_slope'))
MetaDxlController.__init__(self, io, motors, controllers)
class LightDxlController(MetaDxlController):
def __init__(self, io, motors):
controllers = [
PosSpeedLoadDxlController(io, motors, 50.),
AngleLimitRegisterController(io, motors, 10., True),
DxlController(io, motors, 10., True, 'get', 'present_voltage'),
DxlController(io, motors, 10., True, 'get', 'present_temperature')
]
pid_motors = [m for m in motors
if (m.model.startswith('MX') or
m.model.startswith('XL-320'))]
if pid_motors:
controllers.insert(0, DxlController(io, pid_motors, 10., True,
'set', 'pid_gain', 'pid'))
margin_slope_motors = [m for m in motors
if (m.model.startswith('AX') or
m.model.startswith('RX'))]
if margin_slope_motors:
controllers.append(DxlController(io, margin_slope_motors, 10., True,
'set', 'compliance_margin'))
controllers.append(DxlController(io, margin_slope_motors, 10., True,
'set', 'compliance_slope'))
led_motors = [m for m in motors if m.model.startswith('XL-320')]
if led_motors:
controllers.append(DxlController(io, led_motors, 5., False,
'set', 'LED_color', 'led'))
MetaDxlController.__init__(self, io, motors, controllers)

214
pypot-master/pypot-master/pypot/kinematics.py Normal file
View File

@ -0,0 +1,214 @@
import numpy
from collections import namedtuple
"""
This module can be used to compute the forward and inverse kinematics for a chain of revolute joints.
It has been largerly inspired by the Matlab Robotics Toolbox.
"""
class Link(namedtuple('Link', ('theta', 'd', 'a', 'alpha'))):
""" Link object as defined by the standard DH representation.
This representation is based on the following information:
:param float theta: angle about previous z from old x to new x
:param float d: offset along previous z to the common normal
:param float a: offset along previous to the common normal
:param float alpha: angle about common normal, from old z axis to new z axis
.. note:: We are only considering revolute joint.
Please refer to http://en.wikipedia.org/wiki/Denavit-Hartenberg_parameters for more details.
"""
def get_transformation_matrix(self, theta):
""" Computes the homogeneous transformation matrix for this link. """
ct = numpy.cos(theta + self.theta)
st = numpy.sin(theta + self.theta)
ca = numpy.cos(self.alpha)
sa = numpy.sin(self.alpha)
return numpy.matrix(((ct, -st * ca, st * sa, self.a * ct),
(st, ct * ca, -ct * sa, self.a * st),
(0, sa, ca, self.d),
(0, 0, 0, 1)))
class Chain(namedtuple('Chain', ('links', 'base', 'tool'))):
""" Chain of Link that can be used to perform forward and inverse kinematics.
:param list links: list of Link that compose the chain
:param base: the base homogeneous transformation matrix
:param tool: the end tool homogeneous transformation matrix
"""
def __new__(cls, links, base=numpy.identity(4), tool=numpy.identity(4)):
return super(Chain, cls).__new__(cls, links, base, tool)
def forward_kinematics(self, q):
""" Computes the homogeneous transformation matrix of the end effector of the chain.
:param vector q: vector of the joint angles (theta 1, theta 2, ..., theta n)
"""
q = numpy.array(q).flatten()
if len(q) != len(self.links):
raise ValueError('q must contain as element as the number of links')
tr = self.base.copy()
l = []
for link, theta in zip(self.links, q):
tr = tr * link.get_transformation_matrix(theta)
l.append(tr)
tr = tr * self.tool
l.append(tr)
return tr, numpy.asarray(l)
def inverse_kinematics(self, end_effector_transformation,
q=None,
max_iter=1000, tolerance=0.05,
mask=numpy.ones(6),
use_pinv=False):
""" Computes the joint angles corresponding to the end effector transformation.
:param end_effector_transformation: the end effector homogeneous transformation matrix
:param vector q: initial estimate of the joint angles
:param int max_iter: maximum number of iteration
:param float tolerance: tolerance before convergence
:param mask: specify the cartesian DOF that will be ignore (in the case of a chain with less than 6 joints).
:rtype: vector of the joint angles (theta 1, theta 2, ..., theta n)
"""
if q is None:
q = numpy.zeros((len(self.links), 1))
q = numpy.matrix(q.reshape(-1, 1))
best_e = numpy.ones(6) * numpy.inf
best_q = None
alpha = 1.0
for _ in range(max_iter):
e = numpy.multiply(transform_difference(self.forward_kinematics(q)[0], end_effector_transformation), mask)
d = numpy.linalg.norm(e)
if d < numpy.linalg.norm(best_e):
best_e = e.copy()
best_q = q.copy()
alpha *= 2.0 ** (1.0 / 8.0)
else:
q = best_q.copy()
e = best_e.copy()
alpha *= 0.5
if use_pinv:
dq = numpy.linalg.pinv(self._jacob0(q)) * e.reshape((-1, 1))
else:
dq = self._jacob0(q).T * e.reshape((-1, 1))
q += alpha * dq
# d = numpy.linalg.norm(dq)
if d < tolerance:
return q
else:
raise ValueError('could not converge d={}'.format(numpy.linalg.norm(best_e)))
def _jacob0(self, q):
Jn = self._jacobn(q)
Rn = rotation_from_transf(self.forward_kinematics(q)[0])
return numpy.concatenate((numpy.concatenate((Rn, numpy.zeros((3, 3))), axis=1),
numpy.concatenate((numpy.zeros((3, 3)), Rn), 1))) * Jn
def _jacobn(self, q):
q = numpy.array(q).flatten()
U = self.tool.copy()
J = numpy.matrix([[]] * 6)
for link, theta in reversed(zip(self.links, q)):
U = link.get_transformation_matrix(theta) * U
d = numpy.matrix((-U[0, 0] * U[1, 3] + U[1, 0] * U[0, 3],
-U[0, 1] * U[1, 3] + U[1, 1] * U[0, 3],
-U[0, 2] * U[1, 3] + U[1, 2] * U[0, 3]))
delta = U[2, 0:3]
J = numpy.concatenate((numpy.concatenate((d, delta), axis=1).T, J), axis=1)
return J
# MARK: - Utility functions
def transform_difference(t1, t2):
t1 = numpy.array(t1)
t2 = numpy.array(t2)
return numpy.concatenate(((t2[0:3, 3] - t1[0:3, 3]).reshape(3),
0.5 * (numpy.cross(t1[0:3, 0], t2[0:3, 0]) + \
numpy.cross(t1[0:3, 1], t2[0:3, 1]) + \
numpy.cross(t1[0:3, 2], t2[0:3, 2])).reshape(3)))
def rotation_from_transf(tm):
return tm[0:3, 0:3]
def translation_from_transf(tm):
return numpy.array(tm[0:3, 3]).reshape(3)
def components_from_transf(tm):
return rotation_from_transf(tm), translation_from_transf(tm)
def transf_from_components(R, T):
return numpy.matrix(numpy.vstack((numpy.hstack((R, T.reshape(3, 1))),
(0, 0, 0, 1))))
def transl(x, y, z):
M = numpy.matrix(numpy.identity(4))
M[0:3, 3] = numpy.matrix([x, y, z]).T
return M
def trotx(theta):
ct = numpy.cos(theta)
st = numpy.sin(theta)
R = numpy.matrix(((1, 0, 0),
(0, ct, -st),
(0, st, ct)))
return transf_from_components(R, numpy.zeros(3))
def troty(theta):
ct = numpy.cos(theta)
st = numpy.sin(theta)
R = numpy.matrix(((ct, 0, st),
(0, 1, 0),
(-st, 0, ct)))
return transf_from_components(R, numpy.zeros(3))
def trotz(theta):
ct = numpy.cos(theta)
st = numpy.sin(theta)
R = numpy.matrix(((ct, -st, 0),
(st, ct, 0),
(0, 0, 1)))
return transf_from_components(R, numpy.zeros(3))

1
pypot-master/pypot-master/pypot/primitive/__init__.py Normal file
View File

@ -0,0 +1 @@
from .primitive import Primitive, LoopPrimitive

81
pypot-master/pypot-master/pypot/primitive/manager.py Normal file
View File

@ -0,0 +1,81 @@
import logging
import numpy
from collections import defaultdict
from functools import partial
from threading import Lock
from ..utils.stoppablethread import StoppableLoopThread
logger = logging.getLogger(__name__)
class PrimitiveManager(StoppableLoopThread):
""" Combines all :class:`~pypot.primitive.primitive.Primitive` orders and affect them to the real motors.
At a predefined frequency, the manager gathers all the orders sent by the primitive to the "fake" motors, combined them thanks to the filter function and affect them to the "real" motors.
.. note:: The primitives are automatically added (resp. removed) to the manager when they are started (resp. stopped).
"""
def __init__(self, motors, freq=50, filter=partial(numpy.mean, axis=0)):
"""
:param motors: list of real motors used by the attached primitives
:type motors: list of :class:`~pypot.dynamixel.motor.DxlMotor`
:param int freq: update frequency
:param func filter: function used to combine the different request (default mean)
"""
StoppableLoopThread.__init__(self, freq)
self._prim = []
self._motors = motors
self._filter = filter
self.syncing = Lock()
def add(self, p):
""" Add a primitive to the manager. The primitive automatically attached itself when started. """
self._prim.append(p)
def remove(self, p):
""" Remove a primitive from the manager. The primitive automatically remove itself when stopped. """
self._prim.remove(p)
@property
def primitives(self):
""" List of all attached :class:`~pypot.primitive.primitive.Primitive`. """
return self._prim
def update(self):
""" Combined at a predefined frequency the request orders and affect them to the real motors. """
with self.syncing:
for m in self._motors:
to_set = defaultdict(list)
for p in self._prim:
for key, val in getattr(p.robot, m.name)._to_set.items():
to_set[key].append(val)
for key, val in to_set.items():
if key == 'led':
colors = set(val)
if len(colors) > 1:
colors -= {'off'}
filtred_val = colors.pop()
else:
filtred_val = self._filter(val)
logger.debug('Combined %s.%s from %s to %s',
m.name, key, val, filtred_val)
setattr(m, key, filtred_val)
[p._synced.set() for p in self._prim]
def stop(self):
""" Stop the primitive manager. """
for p in self.primitives[:]:
p.stop()
StoppableLoopThread.stop(self)

215
pypot-master/pypot-master/pypot/primitive/move.py Normal file
View File

@ -0,0 +1,215 @@
import json
import time
import logging
import numpy as np
from .primitive import LoopPrimitive
from pypot.utils.interpolation import KDTreeDict
logger = logging.getLogger(__name__)
class Move(object):
""" Simple class used to represent a movement.
This class simply wraps a sequence of positions of specified motors. The sequence must be recorded at a predefined frequency. This move can be recorded through the :class:`~pypot.primitive.move.MoveRecorder` class and played thanks to a :class:`~pypot.primitive.move.MovePlayer`.
"""
def __init__(self, freq):
self._framerate = freq
self._timed_positions = KDTreeDict()
def __repr__(self):
return '<Move framerate={} #keyframes={}>'.format(self.framerate,
len(self.positions()))
def __getitem__(self, i):
return list(self._timed_positions.items())[i]
@property
def framerate(self):
return self._framerate
def add_position(self, pos, time):
""" Add a new position to the movement sequence.
Each position is typically stored as a dict of (time, (motor_name,motor_position)).
"""
self._timed_positions[time] = pos
def iterpositions(self):
""" Returns an iterator on the stored positions. """
return self._timed_positions.items()
def positions(self):
""" Returns a copy of the stored positions. """
return self._timed_positions
def plot(self, ax):
pos = self.positions()
if not pos:
return
motors = list(pos[0].keys())
n = len(pos)
t = np.linspace(0, n / self.framerate, n)
pos = self.positions()
p = {}
for name in motors:
p[name] = []
for tt in t:
for k, v in pos[float(tt)].items():
p[k].append(v[0])
for name in motors:
ax.plot(t, p[name])
ax.legend(motors)
ax.set_xlabel('Time (in s)')
ax.set_ylabel('Position (in degree)')
def save(self, file):
""" Saves the :class:`~pypot.primitive.move.Move` to a json file.
.. note:: The format used to store the :class:`~pypot.primitive.move.Move` is extremely verbose and should be obviously optimized for long moves.
"""
d = {
'framerate': self.framerate,
'positions': self._timed_positions,
}
json.dump(d, file, indent=2)
@classmethod
def create(cls, d):
""" Create a :class:`~pypot.primitive.move.Move` from a dictionary. """
move = cls(d['framerate'])
move._timed_positions.update(d['positions'])
return move
@classmethod
def load(cls, file):
""" Loads a :class:`~pypot.primitive.move.Move` from a json file. """
d = json.load(file)
return cls.create(d)
@classmethod
def loads(cls, str):
""" Loads a :class:`~pypot.primitive.move.Move` from a json string. """
d = json.loads(str)
return cls.create(d)
class MoveRecorder(LoopPrimitive):
""" Primitive used to record a :class:`~pypot.primitive.move.Move`.
The recording can be :meth:`~pypot.primitive.primitive.Primitive.start` and :meth:`~pypot.primitive.primitive.Primitive.stop` by using the :class:`~pypot.primitive.primitive.LoopPrimitive` methods.
.. note:: Re-starting the recording will create a new :class:`~pypot.primitive.move.Move` losing all the previously stored data.
"""
def __init__(self, robot, freq, tracked_motors):
LoopPrimitive.__init__(self, robot, freq)
self.freq = freq
self.tracked_motors = list(map(self.get_mockup_motor, tracked_motors))
self._move = Move(self.freq)
def setup(self):
self._move = Move(self.freq)
def update(self):
position = dict([(m.name, (m.present_position, m.present_speed))
for m in self.tracked_motors])
self._move.add_position(position, self.elapsed_time)
@property
def move(self):
""" Returns the currently recorded :class:`~pypot.primitive.move.Move`. """
return self._move
def add_tracked_motors(self, tracked_motors):
"""Add new motors to the recording"""
new_mockup_motors = list(map(self.get_mockup_motor, tracked_motors))
self.tracked_motors = list(set(self.tracked_motors + new_mockup_motors))
class MovePlayer(LoopPrimitive):
""" Primitive used to play a :class:`~pypot.primitive.move.Move`.
The playing can be :meth:`~pypot.primitive.primitive.Primitive.start` and :meth:`~pypot.primitive.primitive.Primitive.stop` by using the :class:`~pypot.primitive.primitive.LoopPrimitive` methods.
.. warning:: the primitive is run automatically the same framerate than the move record.
The play_speed attribute change only time lockup/interpolation
"""
def __init__(self, robot, move=None, play_speed=1.0, move_filename=None, start_max_speed=50, **kwargs):
self.move = move
self.backwards = False
if move_filename is not None:
with open(move_filename, 'r') as f:
self.move = Move.load(f)
self.play_speed = play_speed if play_speed != 0 and isinstance(play_speed, float) else 1.0
framerate = self.move.framerate if self.move is not None else 50.0
self.start_max_speed = start_max_speed if start_max_speed != 0 else np.inf
for key, value in kwargs.items():
setattr(self, key, value)
LoopPrimitive.__init__(self, robot, framerate)
def setup(self):
if self.move is None:
raise AttributeError("Attribute move is not defined")
self.period = 1.0 / self.move.framerate
self.positions = self.move.positions()
self.__duration = self.duration()
if self.play_speed < 0:
self.play_speed = - self.play_speed
self.backwards = not self.backwards
if self.play_speed == 0:
self.play_speed = 1.0
# Quick fix for limiting too fast movements at the play start
max_goto_time = 0
if self.backwards:
position = self.positions[self.__duration]
else:
position = self.positions[0]
for motor, value in position.items():
motor = getattr(self.robot, motor)
motor.compliant = False
delta_angle = abs(motor.present_position - value[0])
if delta_angle > 5:
goto_time = delta_angle / self.start_max_speed
motor.goto_position(value[0], goto_time)
max_goto_time = goto_time if goto_time > max_goto_time else max_goto_time
time.sleep(max_goto_time)
def update(self):
if self.elapsed_time < self.__duration:
if self.backwards:
position = self.positions[(self.__duration - self.elapsed_time) * self.play_speed]
else:
position = self.positions[self.elapsed_time * self.play_speed]
for motor, value in position.items():
# TODO: Ask pierre if its not a fgi to turn off the compliance
getattr(self.robot, motor).compliant = False
getattr(self.robot, motor).goal_position = value[0]
else:
self.stop()
def duration(self):
if self.move is not None:
return (len(self.move.positions()) / self.move.framerate) / self.play_speed
else:
return 1.0

301
pypot-master/pypot-master/pypot/primitive/primitive.py Normal file
View File

@ -0,0 +1,301 @@
import sys
import numpy
import logging
import threading
from collections import deque
from ..utils import pypot_time as time
from ..utils.stoppablethread import StoppableThread, make_update_loop
from ..utils.trajectory import GotoMinJerk
logger = logging.getLogger(__name__)
class Primitive(StoppableThread):
""" A Primitive is an elementary behavior that can easily be combined to create more complex behaviors.
A primitive is basically a thread with access to a "fake" robot to ensure a sort of sandboxing. More precisely, it means that the primitives will be able to:
* request values from the real robot (motor values, sensors or attached primitives)
* request modification of motor values (those calls will automatically be combined among all primitives by the :class:`~pypot.primitive.manager.PrimitiveManager`).
The syntax of those requests directly match the equivalent code that you could write from the :class:`~pypot.robot.robot.Robot`. For instance you can write::
class MyPrimitive(Primitive):
def run(self):
while True:
for m in self.robot.motors:
m.goal_position = m.present_position + 10
time.sleep(1)
.. warning:: In the example above, while it seems that you are setting a new goal_position, you are only requesting it. In particular, another primitive could request another goal_position and the result will be the combination of both request. For example, if you have two primitives: one setting the goal_position to 10 and the other setting the goal_position to -20, the real goal_position will be set to -5 (by default the mean of all request is used, see the :class:`~pypot.primitive.manager.PrimitiveManager` class for details).
Primitives were developed to allow for the creation of complex behaviors such as walking. You could imagine - and this is what is actually done on the Poppy robot - having one primitive for the walking gait, another for the balance and another for handling falls.
.. note:: This class should always be extended to define your particular behavior in the :meth:`~pypot.primitive.primitive.Primitive.run` method.
"""
methods = ['start', 'stop', 'pause', 'resume']
properties = []
def __init__(self, robot):
""" At instantiation, it automatically transforms the :class:`~pypot.robot.robot.Robot` into a :class:`~pypot.primitive.primitive.MockupRobot`.
.. warning:: You should not directly pass motors as argument to the primitive. If you need to, use the method :meth:`~pypot.primitive.primitive.Primitive.get_mockup_motor` to transform them into "fake" motors. See the :ref:`write_own_prim` section for details.
"""
StoppableThread.__init__(self,
setup=self._prim_setup,
target=self._prim_run,
teardown=self._prim_teardown)
self.robot = MockupRobot(robot)
self._synced = threading.Event()
def _prim_setup(self):
logger.info("Primitive %s setup.", self)
for m in self.robot.motors:
m._to_set.clear()
self.robot._primitive_manager.add(self)
self.setup()
self.t0 = time.time()
def setup(self):
""" Setup methods called before the run loop.
You can override this method to setup the environment needed by your primitive before the run loop. This method will be called every time the primitive is started/restarted.
"""
pass
def _prim_run(self):
self.run()
def run(self):
""" Run method of the primitive thread. You should always overwrite this method.
.. warning:: You are responsible of handling the :meth:`~pypot.utils.stoppablethread.StoppableThread.should_stop`, :meth:`~pypot.utils.stoppablethread.StoppableThread.should_pause` and :meth:`~pypot.utils.stoppablethread.StoppableThread.wait_to_resume` methods correctly so the code inside your run function matches the desired behavior. You can refer to the code of the :meth:`~pypot.utils.stoppablethread.StoppableLoopThread.run` method of the :class:`~pypot.primitive.primitive.LoopPrimitive` as an example.
After termination of the run function, the primitive will automatically be removed from the list of active primitives of the :class:`~pypot.primitive.manager.PrimitiveManager`.
"""
pass
def _prim_teardown(self):
logger.info("Primitive %s teardown.", self)
self.teardown()
# Forces a last synced to make sure that all values sent
# Within the primitives will be sent to the motors.
self._synced.clear()
self._synced.wait()
self.robot._primitive_manager.remove(self)
def teardown(self):
""" Tear down methods called after the run loop.
You can override this method to clean up the environment needed by your primitive. This method will be called every time the primitive is stopped.
"""
pass
@property
def elapsed_time(self):
""" Elapsed time (in seconds) since the primitive runs. """
return time.time() - self.t0
# MARK: - Start/Stop handling
def start(self):
""" Start or restart (the :meth:`~pypot.primitive.primitive.Primitive.stop` method will automatically be called) the primitive. """
if not self.robot._primitive_manager.running:
raise RuntimeError('Cannot run a primitive when the sync is stopped!')
StoppableThread.start(self)
self.wait_to_start()
logger.info("Primitive %s started.", self)
def stop(self, wait=True):
""" Requests the primitive to stop. """
logger.info("Primitive %s stopped.", self)
StoppableThread.stop(self, wait)
def is_alive(self):
""" Determines whether the primitive is running or not.
The value will be true only when the :meth:`~pypot.utils.stoppablethread.StoppableThread.run` function is executed.
"""
return self.running
def get_mockup_motor(self, motor):
""" Gets the equivalent :class:`~pypot.primitive.primitive.MockupMotor`. """
return next((m for m in self.robot.motors if m.name == motor.name), None)
# Utility function to try to help to better control
# the synchronization and merging process of primitives
# This is clearly a patch before a better definition of primitives.
@property
def being_synced(self):
return self.robot._primitive_manager.syncing
def affect_once(self, motor, register, value):
with self.being_synced:
setattr(motor, register, value)
self._synced.clear()
self._synced.wait()
del motor._to_set[register]
class LoopPrimitive(Primitive):
""" Simple primitive that call an update method at a predefined frequency.
You should write your own subclass where you only defined the :meth:`~pypot.primitive.primitive.LoopPrimitive.update` method.
"""
def __init__(self, robot, freq):
Primitive.__init__(self, robot)
# self._period = 1.0 / freq
self.period = 1.0 / freq
self._recent_updates = deque([], 11)
@property
def recent_update_frequencies(self):
""" Returns the 10 most recent update frequencies.
The given frequencies are computed as short-term frequencies!
The 0th element of the list corresponds to the most recent frequency.
"""
return list(reversed([(1.0 / p) for p in numpy.diff(self._recent_updates)]))
def run(self):
""" Calls the :meth:`~pypot.utils.stoppablethread.StoppableLoopThread.update` method at a predefined frequency (runs until stopped). """
make_update_loop(self, self._wrapped_update)
def _wrapped_update(self):
logger.debug('LoopPrimitive %s updated.', self)
self._recent_updates.append(time.time())
self.update()
def update(self):
""" Update methods that will be called at a predefined frequency. """
raise NotImplementedError
class MockupRobot(object):
""" Fake :class:`~pypot.robot.robot.Robot` used by the :class:`~pypot.primitive.primitive.Primitive` to ensure sandboxing. """
def __init__(self, robot):
self._robot = robot
self._motors = []
for a in robot.alias:
setattr(self, a, [])
for m in robot.motors:
mockup_motor = MockupMotor(m)
self._motors.append(mockup_motor)
setattr(self, m.name, mockup_motor)
for a in [a for a in robot.alias if m in getattr(robot, a)]:
getattr(self, a).append(mockup_motor)
def __getattr__(self, attr):
return getattr(self._robot, attr)
def goto_position(self, position_for_motors, duration, control=None, wait=False):
for i, (motor_name, position) in enumerate(position_for_motors.items()):
w = False if i < len(position_for_motors) - 1 else wait
m = getattr(self, motor_name)
m.goto_position(position, duration, control, wait=w)
@property
def motors(self):
""" List of all attached :class:`~pypot.primitive.primitive.MockupMotor`. """
return self._motors
def power_max(self):
for m in self.motors:
m.compliant = False
m.moving_speed = 0
m.torque_limit = 100.0
class MockupMotor(object):
""" Fake Motor used by the primitive to ensure sandboxing:
* the read instructions are directly delegate to the real motor
* the write instructions are stored as request waiting to be combined by the primitive manager.
"""
def __init__(self, motor):
object.__setattr__(self, '_m', motor)
object.__setattr__(self, '_to_set', {})
def __getattr__(self, attr):
return getattr(self._m, attr)
def __setattr__(self, attr, val):
if attr == 'goal_speed':
MockupMotor.goal_speed.fset(self, val)
else:
self._to_set[attr] = val
logger.debug("Setting MockupMotor '%s.%s' to %s",
self.name, attr, val)
def goto_position(self, position, duration, control=None, wait=False):
""" Automatically sets the goal position and the moving speed to reach the desired position within the duration. """
if control is None:
control = self.goto_behavior
if control == 'minjerk':
goto_min_jerk = GotoMinJerk(self, position, duration)
goto_min_jerk.start()
if wait:
goto_min_jerk.wait_to_stop()
elif control == 'dummy':
dp = abs(self.present_position - position)
speed = (dp / float(duration)) if duration > 0 else numpy.inf
self.moving_speed = speed
self.goal_position = position
if wait:
time.sleep(duration)
@property
def goal_speed(self):
""" Goal speed (in degrees per second) of the motor.
This property can be used to control your motor in speed. Setting a goal speed will automatically change the moving speed and sets the goal position as the angle limit.
.. note:: The motor will turn until reaching the angle limit. But this is not a wheel mode, so the motor will stop at its limits.
"""
return numpy.sign(self.goal_position) * self.moving_speed
@goal_speed.setter
def goal_speed(self, value):
if abs(value) < sys.float_info.epsilon:
self.goal_position = self.present_position
else:
# 0.7 corresponds approx. to the min speed that will be converted into 0
# and as 0 corresponds to setting the max speed, we have to check this case
value = numpy.sign(value) * 0.7 if abs(value) < 0.7 else value
self.goal_position = numpy.sign(value) * self.max_pos
self.moving_speed = abs(value)

137
pypot-master/pypot-master/pypot/primitive/utils.py Normal file
View File

@ -0,0 +1,137 @@
import numpy
from copy import deepcopy
from collections import defaultdict
from .primitive import Primitive, LoopPrimitive
class Sinus(LoopPrimitive):
""" Apply a sinus on the motor specified as argument. Parameters (amp, offset and phase) should be specified in degree. """
properties = LoopPrimitive.properties + ['frequency', 'amplitude', 'offset', 'phase']
def __init__(self, robot, refresh_freq,
motor_list,
amp=1, freq=0.5, offset=0, phase=0):
LoopPrimitive.__init__(self, robot, refresh_freq)
self._freq = freq
self._amp = amp
self._offset = offset
self._phase = phase
self.motor_list = [self.get_mockup_motor(m) for m in motor_list]
def update(self):
""" Compute the sin(t) where t is the elapsed time since the primitive has been started. """
pos = self._amp * numpy.sin(self._freq * 2.0 * numpy.pi * self.elapsed_time +
self._phase * numpy.pi / 180.0) + self._offset
for m in self.motor_list:
m.goal_position = pos
@property
def frequency(self):
return self._freq
@frequency.setter
def frequency(self, new_freq):
self._freq = new_freq
@property
def amplitude(self):
return self._amp
@amplitude.setter
def amplitude(self, new_amp):
self._amp = new_amp
@property
def offset(self):
return self._offset
@offset.setter
def offset(self, new_offset):
self._offset = new_offset
@property
def phase(self):
return self._phase
@phase.setter
def phase(self, new_phase):
self._phase = new_phase
class Cosinus(Sinus):
""" Apply a cosinus on the motor specified as argument. Parameters (amp, offset and phase) should be specified in degree. """
def __init__(self, robot, refresh_freq,
motor_list,
amp=1, freq=0.5, offset=0, phase=0):
Sinus.__init__(self, robot, refresh_freq,
motor_list,
amp, freq, offset, phase=(numpy.pi / 2 + phase))
class PositionWatcher(LoopPrimitive):
def __init__(self, robot, refresh_freq, watched_motors):
LoopPrimitive.__init__(self, robot, refresh_freq)
self._pos = defaultdict(list)
self.watched_motors = watched_motors
self._duration = 0.
@property
def record_positions(self):
return deepcopy(self._pos)
def setup(self):
self._pos.clear()
def update(self):
for m in self.watched_motors:
self._pos[m.name].append(m.present_position)
self._duration = self.elapsed_time
def plot(self, ax):
for m, pos in self._pos.items():
t = numpy.linspace(0, self._duration, len(pos))
ax.plot(t, pos)
ax.set_ylabel('position (in deg)')
ax.set_xlabel('time (in s)')
ax.legend(list(self._pos.keys()), loc='best')
class SimplePosture(Primitive):
def __init__(self, robot, duration):
Primitive.__init__(self, robot)
self.duration = duration
def setup(self):
self._speeds = {m: m.moving_speed for m in self.robot.motors}
if hasattr(self, 'leds'):
for m, c in self.leds.items():
m.led = c
def run(self):
if not hasattr(self, 'target_position'):
raise NotImplementedError('You have to define "target_position" first!')
for m in self.robot.motors:
m.compliant = False
self.robot.goto_position(self.target_position, self.duration, wait=True)
def teardown(self):
for m, s in self._speeds.items():
m.moving_speed = s
if hasattr(self, 'leds'):
for m, c in self.leds.items():
m.led = 'off'

12
pypot-master/pypot-master/pypot/robot/__init__.py Normal file
View File

@ -0,0 +1,12 @@
from .robot import Robot
from .config import from_config, from_json, use_dummy_robot
try:
from .remote import from_remote
except ImportError:
pass
try:
from ..vrep import from_vrep
except (ImportError, OSError):
pass

377
pypot-master/pypot-master/pypot/robot/config.py Normal file
View File

@ -0,0 +1,377 @@
"""
The config module allows the definition of the structure of your robot.
Configuration are written as Python dictionary so you can define/modify them programmatically. You can also import them form file such as JSON formatted file. In the configuration you have to define:
* controllers: For each defined controller, you can specify the port name, the attached motors and the synchronization mode.
* motors: You specify all motors belonging to your robot. You have to define their id, type, orientation, offset and angle_limit.
* motorgroups: It allows to define alias of group of motors. They can be nested.
"""
import logging
import numpy
import time
import json
from collections import OrderedDict
import pypot.sensor
import pypot.dynamixel
import pypot.dynamixel.io
import pypot.dynamixel.error
import pypot.dynamixel.motor
import pypot.dynamixel.syncloop
from .robot import Robot
from .controller import DummyController
# This logger should always provides the config as extra
logger = logging.getLogger(__name__)
def from_config(config, strict=True, sync=True, use_dummy_io=False, **extra):
""" Returns a :class:`~pypot.robot.robot.Robot` instance created from a configuration dictionnary.
:param dict config: robot configuration dictionary
:param bool strict: make sure that all ports, motors are availaible.
:param bool sync: choose if automatically starts the synchronization loops
For details on how to write such a configuration dictionnary, you should refer to the section :ref:`config_file`.
"""
logger.info('Loading config... ', extra={'config': config})
alias = config['motorgroups']
# Instatiate the different motor controllers
controllers = []
for c_name, c_params in config['controllers'].items():
motor_names = sum([_motor_extractor(alias, name)
for name in c_params['attached_motors']], [])
attached_motors = [motor_from_confignode(config, name)
for name in motor_names]
# at least one of the motor is set as broken
if [m for m in attached_motors if m._broken]:
strict = False
attached_ids = [m.id for m in attached_motors]
if not use_dummy_io:
dxl_io = dxl_io_from_confignode(config, c_params, attached_ids, strict)
check_motor_eprom_configuration(config, dxl_io, motor_names)
logger.info('Instantiating controller on %s with motors %s',
dxl_io.port, motor_names,
extra={'config': config})
syncloop = (c_params['syncloop'] if 'syncloop' in c_params
else 'BaseDxlController')
SyncLoopCls = getattr(pypot.dynamixel.syncloop, syncloop)
c = SyncLoopCls(dxl_io, attached_motors)
controllers.append(c)
else:
controllers.append(DummyController(attached_motors))
try:
robot = Robot(motor_controllers=controllers, sync=sync)
except RuntimeError:
for c in controllers:
c.io.close()
raise
make_alias(config, robot)
# Create all sensors and attached them
try:
if 'sensors' in config and not use_dummy_io:
sensors = []
for s_name in config['sensors'].keys():
if s_name in extra and extra[s_name] == 'dummy':
config['sensors'][s_name]['type'] = 'Dummy{}'.format(s_name.capitalize())
sensor = sensor_from_confignode(config, s_name, robot)
setattr(robot, s_name, sensor)
sensors.append(sensor)
robot.sensors.append(sensor)
[s.start() for s in sensors if hasattr(s, 'start')]
# If anything goes wrong when adding sensors
# We have to make sure we close the robot properly
# Otherwise trying to open it again will fail.
except Exception:
robot.close()
raise
logger.info('Loading complete!',
extra={'config': config})
return robot
def motor_from_confignode(config, motor_name):
params = config['motors'][motor_name]
type = params['type']
if type == 'XL-320':
MotorCls = pypot.dynamixel.motor.DxlXL320Motor
elif type == 'MX-64' or type == 'MX-106':
MotorCls = pypot.dynamixel.motor.DxlMX64106Motor
elif type.startswith('MX'):
MotorCls = pypot.dynamixel.motor.DxlMXMotor
elif type.startswith('AX') or type.startswith('RX'):
MotorCls = pypot.dynamixel.motor.DxlAXRXMotor
elif type.startswith('SR'):
MotorCls = pypot.dynamixel.motor.DxlSRMotor
broken = 'broken' in params and params['broken']
if 'wheel_mode' in params and params['wheel_mode']:
params['angle_limit'] = (0, 0)
m = MotorCls(id=params['id'],
name=motor_name,
model=type,
direct=True if params['orientation'] == 'direct' else False,
offset=params['offset'],
broken=broken,
angle_limit=params['angle_limit'])
logger.info("Instantiating motor '%s' id=%d direct=%s offset=%s",
m.name, m.id, m.direct, m.offset,
extra={'config': config})
return m
def sensor_from_confignode(config, s_name, robot):
args = config['sensors'][s_name]
cls_name = args.pop("type")
if 'need_robot' in args and args.pop('need_robot'):
args['robot'] = robot
SensorCls = getattr(pypot.sensor, cls_name)
return SensorCls(name=s_name, **args)
def dxl_io_from_confignode(config, c_params, ids, strict):
port = c_params['port']
if port == 'auto':
port = pypot.dynamixel.find_port(ids, strict)
logger.info('Found port {} for ids {}'.format(port, ids))
sync_read = c_params['sync_read']
if sync_read == 'auto':
# USB Vendor Product ID "VID:PID=0403:6001" for USB2Dynamixel
# USB Vendor Product ID "VID:PID=16d0:06a7" for USBAX
vendor_pid = pypot.dynamixel.get_port_vendor_info(port)
sync_read = ('PID=0403:6001' in vendor_pid and c_params['protocol'] == 2 or
'PID=16d0:06a7' in vendor_pid)
logger.info('sync_read is {}. Vendor pid = {}'.format(sync_read, vendor_pid))
handler = pypot.dynamixel.error.BaseErrorHandler
DxlIOCls = (pypot.dynamixel.io.Dxl320IO
if 'protocol' in c_params and c_params['protocol'] == 2
else pypot.dynamixel.io.DxlIO)
dxl_io = DxlIOCls(port=port,
use_sync_read=sync_read,
error_handler_cls=handler)
try:
found_ids = dxl_io.scan(ids)
except pypot.dynamixel.io.DxlError:
dxl_io.close()
found_ids = []
if ids != found_ids:
missing_ids = tuple(set(ids) - set(found_ids))
msg = 'Could not find the motors {} on bus {}.'.format(missing_ids,
dxl_io.port)
logger.warning(msg)
if strict:
dxl_io.close()
raise pypot.dynamixel.io.DxlError(msg)
return dxl_io
def check_motor_eprom_configuration(config, dxl_io, motor_names):
""" Change the angles limits depanding on the robot configuration ;
Check if the return delay time is set to 0.
"""
changed_angle_limits = {}
changed_return_delay_time = {}
for name in motor_names:
m = config['motors'][name]
id = m['id']
try:
old_limits = dxl_io.get_angle_limit((id, ))[0]
old_return_delay_time = dxl_io.get_return_delay_time((id, ))[0]
except IndexError: # probably a broken motor so we just skip
continue
if old_return_delay_time != 0:
logger.warning("Return delay time of %s changed from %s to 0",
name, old_return_delay_time)
changed_return_delay_time[id] = 0
new_limits = m['angle_limit']
if 'wheel_mode' in m and m['wheel_mode']:
dxl_io.set_wheel_mode([m['id']])
time.sleep(0.5)
else:
# TODO: we probably need a better fix for this.
# dxl_io.set_joint_mode([m['id']])
d = numpy.linalg.norm(numpy.asarray(new_limits) - numpy.asarray(old_limits))
if d > 1:
logger.warning("Limits of '%s' changed from %s to %s",
name, old_limits, new_limits,
extra={'config': config})
changed_angle_limits[id] = new_limits
if changed_angle_limits:
dxl_io.set_angle_limit(changed_angle_limits)
time.sleep(0.5)
if changed_return_delay_time:
dxl_io.set_return_delay_time(changed_return_delay_time)
time.sleep(0.5)
def instatiate_motors(config):
motors = []
for m_name, m_params in config['motors']:
MotorCls = pypot.dynamixel.motor.DxlAXRXMotor
if m_params['type'].startswith('MX-64') or m_params['type'].startswith('MX-106'):
MotorCls = pypot.dynamixel.motor.DxlMX64106Motor
elif m_params['type'].startswith('MX'):
MotorCls = pypot.dynamixel.motor.DxlMXMotor
elif m_params['type'].startswith('SR'):
MotorCls = pypot.dynamixel.DxlSRMotor
m = MotorCls(id=m_params['id'],
name=m_name,
direct=True if m_params['orientation'] == 'direct' else False,
offset=m_params['offset'])
motors.append(m)
logger.info("Instantiating motor '%s' id=%d direct=%s offset=%s",
m.name, m.id, m.direct, m.offset,
extra={'config': config})
return motors
def make_alias(config, robot):
alias = config['motorgroups']
# Create the alias for the motorgroups
for alias_name in alias:
motors = [getattr(robot, name) for name in _motor_extractor(alias, alias_name)]
setattr(robot, alias_name, motors)
robot.alias.append(alias_name)
logger.info("Creating alias '%s' for motors %s",
alias_name, [motor.name for motor in motors],
extra={'config': config})
def from_json(json_file, sync=True, strict=True, use_dummy_io=False, **extra):
""" Returns a :class:`~pypot.robot.robot.Robot` instance created from a JSON configuration file.
For details on how to write such a configuration file, you should refer to the section :ref:`config_file`.
"""
with open(json_file) as f:
config = json.load(f, object_pairs_hook=OrderedDict)
return from_config(config, sync=sync, strict=strict, use_dummy_io=use_dummy_io, **extra)
def use_dummy_robot(json_file):
return from_json(json_file, use_dummy_io=True)
def _motor_extractor(alias, name):
motors = []
if name not in alias:
return [name]
for key in alias[name]:
motors += _motor_extractor(alias, key) if key in alias else [key]
return motors
ergo_robot_config = {
'controllers': {
'my_dxl_controller': {
'sync_read': False,
'attached_motors': ['base', 'tip'],
'port': 'auto'
}
},
'motorgroups': {
'base': ['m1', 'm2', 'm3'],
'tip': ['m4', 'm5', 'm6']
},
'motors': {
'm5': {
'orientation': 'indirect',
'type': 'MX-28',
'id': 15,
'angle_limit': [-90.0, 90.0],
'offset': 0.0
},
'm4': {
'orientation': 'direct',
'type': 'MX-28',
'id': 14,
'angle_limit': [-90.0, 90.0],
'offset': 0.0
},
'm6': {
'orientation': 'indirect',
'type': 'MX-28',
'id': 16,
'angle_limit': [-90.0, 90.0],
'offset': 0.0
},
'm1': {
'orientation': 'direct',
'type': 'MX-28', 'id': 11,
'angle_limit': [-90.0, 90.0],
'offset': 0.0
},
'm3': {
'orientation': 'indirect',
'type': 'MX-28',
'id': 13,
'angle_limit': [-90.0, 90.0],
'offset': 0.0
},
'm2': {
'orientation': 'indirect',
'type': 'MX-28',
'id': 12,
'angle_limit': [-90.0, 90.0],
'offset': 0.0
}
}
}

110
pypot-master/pypot-master/pypot/robot/controller.py Normal file
View File

@ -0,0 +1,110 @@
import time
from math import copysign
from ..utils.stoppablethread import StoppableLoopThread
class AbstractController(StoppableLoopThread):
""" Abstract class for motor/sensor controller.
The controller role is to synchronize the reading/writing of a set of instances with their "hardware" equivalent through an :class:`~pypot.robot.io.AbstractIO` object. It is defined as a :class:`~pypot.utils.stoppablethread.StoppableLoopThread` where each loop update synchronizes values from the "software" objects with their "hardware" equivalent.
To define your Controller, you need to define the :meth:`~pypot.utils.stoppablethread.StoppableLoopThread.update` method. This method will be called at the predefined frequency. An exemple of how to do it can be found in :class:`~pypot.dynamixel.controller.BaseDxlController`.
"""
def __init__(self, io, sync_freq):
"""
:param io: IO used to communicate with the hardware motors
:type io: :class:`~pypot.robot.io.AbstractIO`
:param float sync_freq: synchronization frequency
"""
StoppableLoopThread.__init__(self, sync_freq)
self.io = io
def start(self):
StoppableLoopThread.start(self)
self.wait_to_start()
def close(self):
""" Cleans and closes the controller. """
self.stop()
if self.io is not None:
self.io.close()
class MotorsController(AbstractController):
""" Abstract class for motors controller.
The controller synchronizes the reading/writing of a set of motor instances with their "hardware". Each update loop synchronizes values from the "software" :class:`~pypot.dynamixel.motor.DxlMotor` with their "hardware" equivalent.
"""
def __init__(self, io, motors, sync_freq=50):
"""
:param io: IO used to communicate with the hardware motors
:type io: :class:`~pypot.robot.io.AbstractIO`
:param list motors: list of motors attached to the controller
:param float sync_freq: synchronization frequency
"""
AbstractController.__init__(self, io, sync_freq)
self.motors = motors
class DummyController(MotorsController):
def __init__(self, motors):
MotorsController.__init__(self, None, motors)
self.max_speed = 360.0 # degree per second.
def setup(self):
self.last_update = time.time()
for m in self.motors:
m.__dict__['moving_speed'] = 0.0
m.__dict__['present_position'] = 0.0
m.__dict__['goal_position'] = 0.0
def update(self):
delta_t = time.time() - self.last_update
for m in self.motors:
# acceleration infinite, present_speed always equal moving_speed
delta_pos = m.__dict__['goal_position'] - m.__dict__['present_position'] # degree
# degree par second, assumed absolute
speed = (m.__dict__['moving_speed']
if m.__dict__['moving_speed'] != 0.0 else
self.max_speed)
delta_pos_effective = copysign(speed * delta_t, delta_pos)
if abs(delta_pos_effective) >= abs(delta_pos):
m.__dict__['present_position'] = m.__dict__['goal_position']
else:
m.__dict__['present_position'] += delta_pos_effective
self.last_update = time.time()
class SensorsController(AbstractController):
""" Abstract class for sensors controller.
The controller frequently pulls new data from a "real" sensor and updates its corresponding software instance.
"""
def __init__(self, io, sensors, sync_freq=50.):
"""
:param io: IO used to communicate with the hardware motors
:type io: :class:`~pypot.robot.io.AbstractIO`
:param list sensors: list of sensors attached to the controller
:param float sync_freq: synchronization frequency
"""
AbstractController.__init__(self, io, sync_freq)
self.sensors = sensors

10
pypot-master/pypot-master/pypot/robot/io.py Normal file
View File

@ -0,0 +1,10 @@
from abc import ABCMeta, abstractmethod
class AbstractIO(object, metaclass=ABCMeta):
""" AbstractIO class which handles communication with "hardware" motors. """
@abstractmethod
def close(self):
""" Clean and close the IO connection. """
pass

11
pypot-master/pypot-master/pypot/robot/motor.py Normal file
View File

@ -0,0 +1,11 @@
class Motor(object):
""" Purely abstract class representing any motor object. """
registers = []
def __init__(self, name):
self._name = name
@property
def name(self):
return self._name

68
pypot-master/pypot-master/pypot/robot/remote.py Normal file
View File

@ -0,0 +1,68 @@
import zerorpc
class RemoteRobotClient(object):
""" Remote Access to a Robot through the REST API.
This RemoteRobot gives you access to motors and alias.
For each motor you can read/write all of their registers.
You also have access to primitives.
More specifically you can start/stop them.
"""
def __init__(self, host, port):
client = zerorpc.Client()
client.connect('tcp://{}:{}'.format(host, port))
self.motors = []
for name in client.get_motors_list():
class Register(object):
def __init__(self, motorname, regname):
self.motorname = motorname
self.regname = regname
def __get__(self, instance, owner):
return client.get_register_value(self.motorname, self.regname)
def __set__(self, instance, value):
client.set_register_value(self.motorname, self.regname, value)
class Motor(object):
def __repr__(self):
return ('<Motor name={self.name} '
'id={self.id} '
'pos={self.present_position}>').format(self=self)
for reg in client.get_registers_list(name):
setattr(Motor, reg.decode(), Register(name, reg))
m = Motor()
setattr(self, m.name, m)
self.motors.append(m)
for alias in client.get_motors_alias():
motors = [getattr(self, name) for name in client.get_motors_list(alias)]
setattr(self, alias, motors)
class Primitive(object):
def __init__(self, name):
self.name = name
def start(self):
client.start_primitive(self.name)
def stop(self):
client.stop_primitive(self.name)
self.primitives = []
for p in client.get_primitives_list():
prim = Primitive(p)
setattr(self, p.decode(), prim)
self.primitives.append(prim)
def from_remote(host, port):
""" Remote access to a Robot through the REST API. """
return RemoteRobotClient(host, port)

187
pypot-master/pypot-master/pypot/robot/robot.py Normal file
View File

@ -0,0 +1,187 @@
import logging
from ..primitive.manager import PrimitiveManager
logger = logging.getLogger(__name__)
class Robot(object):
""" This class is used to regroup all motors and sensors of your robots.
Most of the time, you do not want to directly instantiate this class, but you rather want to use a factory which creates a robot instance - e.g. from a python dictionnary (see :ref:`config_file`).
This class encapsulates the different controllers (such as dynamixel ones) that automatically synchronize the virtual sensors/effectors instances held by the robot class with the real devices. By doing so, each sensor/effector can be synchronized at a different frequency.
This class also provides a generic motors accessor in order to (more or less) easily extends this class to other types of motor.
"""
def __init__(self, motor_controllers=[], sensor_controllers=[], sync=True):
"""
:param list motor_controllers: motors controllers to attach to the robot
:param list sensor_controllers: sensors controllers to attach to the robot
:param bool sync: choose if automatically starts the synchronization loops
"""
self._motors = []
self._sensors = []
self.alias = []
self._controllers = sensor_controllers + motor_controllers
for controller in motor_controllers:
for m in controller.motors:
setattr(self, m.name, m)
self._motors.extend(controller.motors)
for controller in sensor_controllers:
for s in controller.sensors:
setattr(self, s.name, s)
self._sensors.extend(controller.sensors)
self._attached_primitives = {}
self._primitive_manager = PrimitiveManager(self.motors)
self._syncing = False
if sync:
self.start_sync()
def close(self):
""" Cleans the robot by stopping synchronization and all controllers."""
self.stop_sync()
[c.io.close() for c in self._controllers if c.io is not None]
def __repr__(self):
return '<Robot motors={}>'.format(self.motors)
def start_sync(self):
""" Starts all the synchonization loop (sensor/effector controllers). """
if self._syncing:
return
[c.start() for c in self._controllers]
[c.wait_to_start() for c in self._controllers]
self._primitive_manager.start()
self._primitive_manager._running.wait()
self._syncing = True
logger.info('Starting robot synchronization.')
def stop_sync(self):
""" Stops all the synchonization loop (sensor/effector controllers). """
if not self._syncing:
return
if self._primitive_manager.running:
self._primitive_manager.stop()
[c.stop() for c in self._controllers]
[s.close() for s in self.sensors if hasattr(s, 'close')]
self._syncing = False
logger.info('Stopping robot synchronization.')
def attach_primitive(self, primitive, name):
setattr(self, name, primitive)
self._attached_primitives[name] = primitive
primitive.name = name
logger.info("Attaching primitive '%s' to the robot.", name)
@property
def motors(self):
""" Returns all the motors attached to the robot. """
return self._motors
@property
def sensors(self):
""" Returns all the sensors attached to the robot. """
return self._sensors
@property
def active_primitives(self):
""" Returns all the primitives currently running on the robot. """
return self._primitive_manager._prim
@property
def primitives(self):
""" Returns all the primitives name attached to the robot. """
return list(self._attached_primitives.values())
@property
def compliant(self):
""" Returns a list of all the compliant motors. """
return [m for m in self.motors if m.compliant]
@compliant.setter
def compliant(self, is_compliant):
""" Switches all motors to compliant (resp. non compliant) mode. """
for m in self.motors:
m.compliant = is_compliant
def goto_position(self, position_for_motors, duration, control=None, wait=False):
""" Moves a subset of the motors to a position within a specific duration.
:param dict position_for_motors: which motors you want to move {motor_name: pos, motor_name: pos,...}
:param float duration: duration of the move
:param str control: control type ('dummy', 'minjerk')
:param bool wait: whether or not to wait for the end of the move
.. note::In case of dynamixel motors, the speed is automatically adjusted so the goal position is reached after the chosen duration.
"""
for i, (motor_name, position) in enumerate(position_for_motors.items()):
w = False if i < len(position_for_motors) - 1 else wait
m = getattr(self, motor_name)
m.goto_position(position, duration, control, wait=w)
def power_up(self):
""" Changes all settings to guarantee the motors will be used at their maximum power. """
for m in self.motors:
m.compliant = False
m.moving_speed = 0
m.torque_limit = 100.0
def to_config(self):
""" Generates the config for the current robot.
.. note:: The generated config should be used as a basis and must probably be modified.
"""
from ..dynamixel.controller import DxlController
dxl_controllers = [c for c in self._controllers
if isinstance(c, DxlController)]
config = {}
config['controllers'] = {}
for i, c in enumerate(dxl_controllers):
name = 'dxl_controller_{}'.format(i)
config['controllers'][name] = {
'port': c.io.port,
'sync_read': c.io._sync_read,
'attached_motors': [m.name for m in c.motors],
}
config['motors'] = {}
for m in self.motors:
config['motors'][m.name] = {
'id': m.id,
'type': m.model,
'offset': m.offset,
'orientation': 'direct' if m.direct else 'indirect',
'angle_limit': m.angle_limit,
}
if m.angle_limit == (0, 0):
config['motors']['wheel_mode'] = True
config['motorgroups'] = {}
return config

39
pypot-master/pypot-master/pypot/robot/sensor.py Normal file
View File

@ -0,0 +1,39 @@
from numpy import array, zeros
class Sensor(object):
""" Purely abstract class representing any sensor object. """
registers = []
def __init__(self, name):
self._name = name
@property
def name(self):
return self._name
class ObjectTracker(Sensor):
registers = Sensor.registers + ['position', 'orientation']
def __init__(self, name):
Sensor.__init__(self, name)
self._pos = zeros(3)
self._ori = zeros(3)
@property
def position(self):
return self._pos
@position.setter
def position(self, new_pos):
self._pos = array(new_pos)
@property
def orientation(self):
return self._pos
@orientation.setter
def orientation(self, new_ori):
self._ori = array(new_ori)

5
pypot-master/pypot-master/pypot/sensor/__init__.py Normal file
View File

@ -0,0 +1,5 @@
from .depth import *
from .camera import *
from .contact import *
from .imagefeature import *
from .arduino import *

5
pypot-master/pypot-master/pypot/sensor/arduino/__init__.py Normal file
View File

@ -0,0 +1,5 @@
try:
from .arduino_sensor import ArduinoSensor
except ImportError:
pass

54
pypot-master/pypot-master/pypot/sensor/arduino/arduino_sensor.py Normal file
View File

@ -0,0 +1,54 @@
import serial
import json
from ...robot.sensor import Sensor
from ...utils import StoppableLoopThread
class ArduinoSensor(Sensor):
""" Give access to arduino sensor.
Here it is an example of the arduino code to retrieve the time:
unsigned long time;
void setup() {
Serial.begin(1000000);
}
void loop() {
// prints fixed data in json format
Serial.print("{\"Day\":\"monday\",");
Serial.print("\"Time\":");
time = millis();
// prints time since program started
Serial.print(time);
Serial.println("}");
// wait 20 ms to send the data at 50 Hz
delay(0.02);
}
Be careful to not set the sync_freq of your controller
slower than the data comes from your arduino (here 50 Hz).
"""
def __init__(self, name, port, baud, sync_freq=50.0):
Sensor.__init__(self, name)
self.port = port
self.baud = baud
self._controller = StoppableLoopThread(sync_freq, update=self.update)
def start(self):
self._ser = serial.Serial(self.port, self.baud)
self._line = ''
self._controller.start()
def close(self):
self._controller.stop()
self._ser.close()
def update(self):
while self._ser.inWaiting() > 0:
self._line = self._ser.readline().decode()
try:
self.sensor_dict = json.loads(self._line)
except ValueError:
pass

9
pypot-master/pypot-master/pypot/sensor/camera/__init__.py Normal file
View File

@ -0,0 +1,9 @@
from ...robot.controller import SensorsController
from .dummy import DummyCamera
try:
from .opencvcam import OpenCVCamera
except ImportError:
pass

52
pypot-master/pypot-master/pypot/sensor/camera/abstractcam.py Normal file
View File

@ -0,0 +1,52 @@
import time
from threading import Thread
from ...robot.sensor import Sensor
class AbstractCamera(Sensor):
registers = Sensor.registers + ['frame', 'resolution', 'fps']
def __init__(self, name, resolution, fps):
Sensor.__init__(self, name)
self._res, self._fps = resolution, fps
self._last_frame = self._grab_and_process()
self.running = True
self._processing = Thread(target=self._process_loop)
self._processing.daemon = True
self._processing.start()
@property
def frame(self):
return self._last_frame
def post_processing(self, image):
return image
def grab(self):
raise NotImplementedError
def _grab_and_process(self):
return self.post_processing(self.grab())
def _process_loop(self):
period = 1.0 / self.fps
while self.running:
self._last_frame = self._grab_and_process()
time.sleep(period)
@property
def resolution(self):
return list(reversed(self.frame.shape[:2]))
@property
def fps(self):
return self._fps
def close(self):
self.running = False
self._processing.join()

14
pypot-master/pypot-master/pypot/sensor/camera/dummy.py Normal file
View File

@ -0,0 +1,14 @@
import numpy
from .abstractcam import AbstractCamera
class DummyCamera(AbstractCamera):
def __init__(self, name, resolution, fps, **extra):
AbstractCamera.__init__(self, name, resolution, fps)
def grab(self):
if not hasattr(self, '_frame'):
self._frame = numpy.zeros(list(self._res) + [3], dtype=numpy.uint8)
return self._frame

34
pypot-master/pypot-master/pypot/sensor/camera/opencvcam.py Normal file
View File

@ -0,0 +1,34 @@
import cv2
from .abstractcam import AbstractCamera
class OpenCVCamera(AbstractCamera):
registers = AbstractCamera.registers + ['index']
def __init__(self, name, index, fps, resolution=None):
self._index = index
self.capture = cv2.VideoCapture(self.index)
if not self.capture.isOpened():
raise ValueError('Can not open camera device {}. You should start your robot with argument camera=\'dummy\'. E.g. p = PoppyErgoJr(camera=\'dummy\')'.format(index))
if resolution is not None:
self.capture.set(cv2.CAP_PROP_FRAME_WIDTH, resolution[0])
self.capture.set(cv2.CAP_PROP_FRAME_HEIGHT, resolution[1])
AbstractCamera.__init__(self, name, resolution, fps)
@property
def index(self):
return self._index
def grab(self):
rval, frame = self.capture.read()
if not rval:
raise EnvironmentError('Can not grab image from the camera!')
return frame
def close(self):
AbstractCamera.close(self)
self.capture.release()

4
pypot-master/pypot-master/pypot/sensor/contact/__init__.py Normal file
View File

@ -0,0 +1,4 @@
try:
from .contact import ContactSensor
except ImportError:
pass

25
pypot-master/pypot-master/pypot/sensor/contact/contact.py Normal file
View File

@ -0,0 +1,25 @@
import RPi.GPIO as GPIO
from ...robot.sensor import Sensor
class ContactSensor(Sensor):
""" Gives access to a micro switch sensor. """
registers = Sensor.registers + ['contact']
def __init__(self, name, gpio_data, gpio_vcc=None):
Sensor.__init__(self, name)
self._pin = gpio_data
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(self._pin, GPIO.IN)
if gpio_vcc is not None:
self._vcc = gpio_vcc
GPIO.setup(self._vcc, GPIO.OUT)
GPIO.output(self._vcc, GPIO.HIGH)
@property
def contact(self):
return GPIO.input(self._pin) != 0

5
pypot-master/pypot-master/pypot/sensor/depth/__init__.py Normal file
View File

@ -0,0 +1,5 @@
try:
from .sonar import SonarSensor
except ImportError:
pass

134
pypot-master/pypot-master/pypot/sensor/depth/sonar.py Normal file
View File

@ -0,0 +1,134 @@
import time
import numpy
from collections import deque
from ...robot.sensor import Sensor
from ...utils import StoppableLoopThread
from ...utils.i2c_controller import I2cController
class SonarSensor(Sensor):
""" Give access to ultrasonic I2C modules SRF-02 in a *pypot way*
It provides one register: distance (in meters).
"""
registers = Sensor.registers + ['distance']
def __init__(self, name, i2c_pin, address, sync_freq=50.0):
Sensor.__init__(self, name)
self._d = numpy.nan
self._sonar = Sonar(i2c_pin, [address])
self._controller = StoppableLoopThread(sync_freq, update=self.update)
self._controller.start()
def close(self):
self._controller.stop()
def update(self):
self._sonar.update()
self.distance = self._sonar.data[0]
@property
def distance(self):
return self._d
@distance.setter
def distance(self, d):
self._d = d / 100
class Sonar(object):
""" Give access to ultrasonic I2C modules SRF-02 connected with I2C pin of your board.
To get more information, go to http://www.robot-electronics.co.uk/htm/srf02techI2C.htm
Example:
> i2c = smbus.SMBus(1)
> sonar = Sonar(i2c, addresses=[0x70, 0x71, 0x72])
>
"""
def __init__(self, pin_number, addresses=[0x70]):
""" 0x70 is the default address for the SRF-02 I2C module. """
self.i2c = I2cController(pin_number)
self.addresses = addresses
self.data = None
self._raw_data_queues = [deque([], 5) for _ in addresses]
self.results_type = {'inches': 0x50,
'centimeters': 0x51,
'microseconds': 0x52}
self.__errors = 0
def update(self):
self.ping()
time.sleep(0.065)
self.data = self._filter(self.read())
return self.data
def ping(self):
for addr in self.addresses:
self._ping(addr)
def read(self, reg=2):
return [self._read(addr, reg) for addr in self.addresses]
def _filter(self, data):
""" Apply a filter to reduce noisy data.
Return the median value of a heap of data.
"""
filtered_data = []
for queue, data in zip(self._raw_data_queues, data):
queue.append(data)
filtered_data.append(numpy.median(queue))
return filtered_data
def _ping(self, address, data=None):
d = data if data is not None else self.results_type['centimeters']
while True:
try:
self.i2c.write_byte_data(address, 0, d)
break
except IOError:
time.sleep(0.005)
self.__errors += 1
def _read(self, address, reg=2):
while True:
try:
return int(self.i2c.read_word_data(address, reg)) / 256
except IOError:
time.sleep(0.005)
self.__errors += 1
if __name__ == '__main__':
import smbus
from pylab import plot
i2c = smbus.SMBus(1)
sonar = Sonar(i2c)
d = []
t = [time.time()]
for _ in range(1000):
sonar.update()
d.append(sonar.data[0])
t.append(time.time() - t[0])
plot(t[1:], d)

14
pypot-master/pypot-master/pypot/sensor/imagefeature/__init__.py Normal file
View File

@ -0,0 +1,14 @@
try:
from .marker import MarkerDetector
except ImportError:
pass
try:
from .blob import BlobDetector
except ImportError:
pass
try:
from .face import FaceDetector
except ImportError:
pass

82
pypot-master/pypot-master/pypot/sensor/imagefeature/blob.py Normal file
View File

@ -0,0 +1,82 @@
import cv2
from numpy import ones, uint8, concatenate
from ...robot.controller import SensorsController
from ...robot.sensor import Sensor
class Blob(Sensor):
registers = Sensor.registers + ['center', 'radius']
def __init__(self, x, y, radius):
self.center = x, y
self.radius = radius
def draw(self, img, color=(255, 0, 0), thickness=3):
cv2.circle(img, self.center, self.radius, color, thickness)
@property
def json(self):
return {"center": self.center, "radius": self.radius}
class BlobDetector(SensorsController):
channels = {
'R': 2, 'G': 1, 'B': 0,
'H': 0, 'S': 1, 'V': 2
}
def __init__(self, robot, name, cameras, freq, filters):
SensorsController.__init__(self, None, [], freq)
self.name = name
self._robot = robot
self._names = cameras
self._blobs = []
self.filters = filters
def detect_blob(self, img, filters):
"""
"filters" must be something similar to:
filters = {
'R': (150, 255), # (min, max)
'S': (150, 255),
}
"""
acc_mask = ones(img.shape[:2], dtype=uint8) * 255
rgb = img.copy()
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
for c, (min, max) in filters.items():
img = rgb if c in 'RGB' else hsv
mask = img[:, :, self.channels[c]]
mask[mask < min] = 0
mask[mask > max] = 0
acc_mask &= mask
kernel = ones((5, 5), uint8)
acc_mask = cv2.dilate(cv2.erode(acc_mask, kernel), kernel)
circles = cv2.HoughCircles(acc_mask, cv2.HOUGH_GRADIENT, 3, img.shape[0] / 5.)
return circles.reshape(-1, 3) if circles is not None else []
def update(self):
if not hasattr(self, 'cameras'):
self.cameras = [getattr(self._robot, c) for c in self._names]
self._blobs = concatenate([self.detect_blob(c.frame, self.filters)
for c in self.cameras])
@property
def blobs(self):
return [Blob(*b) for b in self._blobs]
@property
def registers(self):
return ['blobs']

57
pypot-master/pypot-master/pypot/sensor/imagefeature/face.py Normal file
View File

@ -0,0 +1,57 @@
import cv2
from numpy import mean, concatenate
from ...robot.controller import SensorsController
from ...robot.sensor import Sensor
class Face(Sensor):
registers = Sensor.registers + ['center', 'rect']
def __init__(self, rect):
x, y, w, h = rect
self.center = mean([[x, x + w], [y, y + h]], axis=1)
self.rect = rect
def draw(self, img, color=(255, 0, 0), thickness=3):
x, y, w, h = self.rect
cv2.rectangle(img, (x, y), (x + w, y + h), color, thickness)
@property
def json(self):
return {"center": self.center, "rect": self.rect}
class FaceDetector(SensorsController):
def __init__(self, robot, name, cameras, freq,
cascade='/home/coyote/dev/opencv-3.0.0/data/haarcascades/haarcascade_frontalface_alt.xml'):
SensorsController.__init__(self, None, [], freq)
self.name = name
self._robot = robot
self._names = cameras
self._faces = []
self.cascade = cv2.CascadeClassifier(cascade)
def detect_face(self, img):
rects = self.cascade.detectMultiScale(img, scaleFactor=1.3,
minNeighbors=4,
minSize=(20, 20))
return rects
def update(self):
if not hasattr(self, 'cameras'):
self.cameras = [getattr(self._robot, c) for c in self._names]
self._faces = concatenate([self.detect_face(c.frame) for c in self.cameras])
@property
def faces(self):
return [Face(f) for f in self._faces]
@property
def registers(self):
return ['faces']

64
pypot-master/pypot-master/pypot/sensor/imagefeature/marker.py Normal file
View File

@ -0,0 +1,64 @@
from multiprocessing import Process, Queue
from hampy import detect_markers
from ...robot.controller import SensorsController
from ...robot.sensor import Sensor
class Marker(Sensor):
registers = Sensor.registers + ['position', 'id']
def __init__(self, marker):
Sensor.__init__(self, 'marker_{}'.format(marker.id))
self.position = marker.normalized_center
self.id = marker.id
self._marker = marker
def __getattr__(self, attr):
return getattr(self._marker, attr)
@property
def json(self):
return {"id": self.id, "position": self.position}
class MarkerDetector(SensorsController):
def __init__(self, robot, name, cameras, freq, multiprocess=True):
SensorsController.__init__(self, None, [], freq)
self.name = name
self._robot = robot
self._names = cameras
self.detect = (lambda img: self._bg_detection(img)
if multiprocess else list(detect_markers(img)))
def update(self):
if not hasattr(self, 'cameras'):
self.cameras = [getattr(self._robot, c) for c in self._names]
self._markers = sum([self.detect(c.frame) for c in self.cameras], [])
self.sensors = [Marker(m) for m in self._markers]
@property
def markers(self):
return self.sensors
@property
def registers(self):
return ['markers']
def _detect(self, q, img):
q.put(list(detect_markers(img)))
def _bg_detection(self, img):
if not hasattr(self, 'q'):
self.q = Queue()
p = Process(target=self._detect, args=(self.q, img))
p.start()
return self.q.get()

4
pypot-master/pypot-master/pypot/sensor/kinect/__init__.py Normal file
View File

@ -0,0 +1,4 @@
try:
from .sensor import KinectSensor
except ImportError:
pass

110
pypot-master/pypot-master/pypot/sensor/kinect/sensor.py Normal file
View File

@ -0,0 +1,110 @@
"""
This code has been developed by Baptiste Busch: https://github.com/buschbapti
This module allows you to retrieve Skeleton information from a Kinect device.
It is only the client side of a zmq client/server application.
The server part can be found at: https://bitbucket.org/buschbapti/kinectserver/src
It used the Microsoft Kinect SDK and thus only work on Windows.
Of course, the client side can be used on any platform.
"""
import zmq
import numpy
import threading
from collections import namedtuple
from ...utils import Point3D, Point2D, Quaternion
torso_joints = ('hip_center', 'spine', 'shoulder_center', 'head')
left_arm_joints = ('shoulder_left', 'elbow_left', 'wrist_left', 'hand_left')
right_arm_joints = ('shoulder_right', 'elbow_right', 'wrist_right', 'hand_right')
left_leg_joints = ('hip_left', 'knee_left', 'ankle_left', 'foot_left')
right_leg_joints = ('hip_right', 'knee_right', 'ankle_right', 'foot_right')
skeleton_joints = torso_joints + left_arm_joints + right_arm_joints + left_leg_joints + right_leg_joints
class Skeleton(namedtuple('Skeleton', ('timestamp', 'user_id') + skeleton_joints)):
joints = skeleton_joints
Joint = namedtuple('Joint', ('position', 'orientation', 'pixel_coordinate'))
class KinectSensor(object):
def __init__(self, addr, port):
self._lock = threading.Lock()
self._skeleton = {}
self.context = zmq.Context()
self.sub_skel = self.context.socket(zmq.SUB)
self.sub_skel.connect('tcp://{}:{}'.format(addr, port))
self.sub_skel.setsockopt(zmq.SUBSCRIBE, '')
t = threading.Thread(target=self.get_skeleton)
t.daemon = True
t.start()
def remove_user(self, user_index):
with self._lock:
del self._skeleton[user_index]
def remove_all_users(self):
with self._lock:
self._skeleton = {}
@property
def tracked_skeleton(self):
with self._lock:
return self._skeleton
@tracked_skeleton.setter
def tracked_skeleton(self, skeleton):
with self._lock:
self._skeleton[skeleton.user_id] = skeleton
def get_skeleton(self):
while True:
md = self.sub_skel.recv_json()
msg = self.sub_skel.recv()
skel_array = numpy.fromstring(msg, dtype=float, sep=",")
skel_array = skel_array.reshape(md['shape'])
nb_joints = md['shape'][0]
joints = []
for i in range(nb_joints):
x, y, z, w = skel_array[i][0:4]
position = Point3D(x / w, y / w, z / w)
pixel_coord = Point2D(*skel_array[i][4:6])
orientation = Quaternion(*skel_array[i][6:10])
joints.append(Joint(position, orientation, pixel_coord))
self.tracked_skeleton = Skeleton(md['timestamp'], md['user_index'], *joints)
def run(self):
cv2.startWindowThread()
while True:
img = numpy.zeros((480, 640, 3))
skeleton = kinect.tracked_skeleton
if skeleton:
for user, skel in skeleton.items():
for joint_name in skel.joints:
x, y = getattr(skel, joint_name).pixel_coordinate
pt = (int(x), int(y))
cv2.circle(img, pt, 5, (255, 255, 255), thickness=-1)
kinect.remove_all_users()
cv2.imshow('Skeleton', img)
cv2.waitKey(50)
self.sub_skel.close()
self.context.term()
if __name__ == '__main__':
import cv2
kinect = KinectSensor('193.50.110.177', 9999)
kinect.run()

57
pypot-master/pypot-master/pypot/sensor/optibridge.py Normal file
View File

@ -0,0 +1,57 @@
import zmq
import time
import pickle
import threading
from . import optitrack
class OptiBridgeServer(threading.Thread):
def __init__(self, bridge_host, bridge_port,
opti_addr, opti_port, obj_name):
threading.Thread.__init__(self)
self.daemon = True
c = zmq.Context()
self.s = c.socket(zmq.PUB)
self.s.bind('tcp://{}:{}'.format(bridge_host, bridge_port))
self.optitrack = optitrack.OptiTrackClient(opti_addr, opti_port, obj_name)
self.optitrack.start()
self.obj_name = obj_name
def run(self):
while True:
self.s.send(pickle.dumps(self.optitrack.recent_tracked_objects))
time.sleep(0.02)
class OptiTrackClient(threading.Thread):
def __init__(self, bridge_host, bridge_port, obj_name):
threading.Thread.__init__(self)
self.daemon = True
c = zmq.Context()
self.s = c.socket(zmq.SUB)
self.s.connect('tcp://{}:{}'.format(bridge_host, bridge_port))
self.s.setsockopt(zmq.SUBSCRIBE, '')
self.obj_name = obj_name
self._tracked_obj = {}
def run(self):
while True:
d = pickle.loads(self.s.recv())
self._tracked_obj = {
k: d[k]
for k in [k for k in list(d.keys()) if k in self.obj_name]
}
@property
def tracked_objects(self):
return self._tracked_obj
@property
def recent_tracked_objects(self):
return self.tracked_objects

83
pypot-master/pypot-master/pypot/sensor/optitrack.py Normal file
View File

@ -0,0 +1,83 @@
import time
import numpy
import datetime
import threading
from collections import namedtuple
TrackedObject = namedtuple('TrackedObject', ('position', 'quaternion', 'orientation', 'timestamp'))
def quat2euler(q):
qx, qy, qz, qw = q
sqx, sqy, sqz, sqw = q ** 2
invs = 1.0 / (sqx + sqy + sqz + sqw)
yaw = numpy.arctan2(2.0 * (qx * qz + qy * qw) * invs, (sqx - sqy - sqz + sqw) * invs)
pitch = -numpy.arcsin(2.0 * (qx * qy - qz * qw) * invs)
roll = numpy.arctan2(2.0 * (qy * qz + qx * qw) * invs, (-sqx + sqy - sqz + sqw) * invs)
return numpy.array((yaw, pitch, roll))
try:
import vrpn
class OptiTrackClient(threading.Thread):
""" Retrieves position, orientation, and timestamp of each tracked object.
The position is expressed in meters (X is left, Y is up, and Z is depth).
The orientation is expressed in radians (yaw, pitch, roll).
"""
def __init__(self, addr, port, obj_names):
threading.Thread.__init__(self)
self.daemon = True
self.trackers = []
for obj in obj_names:
t = vrpn.receiver.Tracker('{}@{}:{}'.format(obj, addr, port))
t.register_change_handler(obj, self.handler, 'position')
self.trackers.append(t)
self._tracked_objects = {}
@property
def tracked_objects(self):
return self._tracked_objects
@property
def recent_tracked_objects(self):
""" Only returns the objects that have been tracked less than 20ms ago. """
dt = 0.02
f = lambda name: (datetime.datetime.now() - self.tracked_objects[name].timestamp).total_seconds()
return dict([(k, v) for k, v in self.tracked_objects.items() if f(k) < dt])
def handler(self, obj, data):
self.tracked_objects[obj] = TrackedObject(numpy.array(*data['position']),
numpy.array(data['quaternion']),
quat2euler(numpy.array(data['quaternion'])),
datetime.datetime.now())
def serve_forever(self):
self.start()
while True:
try:
self.join(timeout=1.0)
except KeyboardInterrupt:
break
def run(self):
while True:
for t in self.trackers:
t.mainloop()
time.sleep(1.0 / 120)
except ImportError:
pass
if __name__ == '__main__':
c = OptiTrackClient('193.50.110.176', 3883, ('obj_1', ))
c.serve_forever()

19
pypot-master/pypot-master/pypot/server/__init__.py Normal file
View File

@ -0,0 +1,19 @@
try:
from .httpserver import HTTPRobotServer
except ImportError:
pass
try:
from .zmqserver import ZMQRobotServer
except ImportError:
pass
try:
from .server import RemoteRobotServer
except ImportError:
pass
try:
from .ws import WsRobotServer
except ImportError:
pass

1136
pypot-master/pypot-master/pypot/server/httpserver.py Normal file
View File

File diff suppressed because it is too large Load Diff

273
pypot-master/pypot-master/pypot/server/rest.py Normal file
View File

@ -0,0 +1,273 @@
import os
from numpy import round
import cv2 # for camera frame
from operator import attrgetter
from pypot.primitive.move import MovePlayer, MoveRecorder, Move
from pathlib import Path
class RESTRobot(object):
""" REST API for a Robot.
Through the REST API you can currently access:
* the motors list (and the aliases)
* the registers list for a specific motor
* read/write a value from/to a register of a specific motor
* the sensors list
* the registers list for a specific motor
* read/write a value from/to a register of a specific motor
* the primitives list (and the active)
* start/stop primitives
"""
def __init__(self, robot):
self.robot = robot
self.moves_path = Path(".")
# Access motor related values
def get_motors_list(self, alias='motors'):
return [m.name for m in getattr(self.robot, alias)]
def get_motor_registers_list(self, motor):
return self._get_register_value(motor, 'registers')
# alias to above method
def get_registers_list(self, motor):
return self.get_motor_registers_list(motor)
def get_motor_register_value(self, motor, register):
return self._get_register_value(motor, register)
# alias to above method
def get_register_value(self, motor, register):
return self.get_motor_register_value(motor, register)
def set_motor_register_value(self, motor, register, value):
self._set_register_value(motor, register, value)
# alias to above method
def set_register_value(self, motor, register, value):
self.set_motor_register_value(motor, register, value)
def get_motors_alias(self):
return self.robot.alias
def set_goto_position_for_motor(self, motor, position, duration, wait=False):
m = getattr(self.robot, motor)
m.goto_position(position, duration, wait=wait)
def set_goto_positions_for_motors(self, motors, positions, duration, control=None, wait=False):
for i, motor_name in enumerate(motors):
w = False if i < len(motors) - 1 else wait
m = getattr(self.robot, motor_name)
m.goto_position(positions[i], duration, control, wait=w)
# Access sensor related values
def get_sensors_list(self):
return [s.name for s in self.robot.sensors]
def get_sensors_registers_list(self, sensor):
return self._get_register_value(sensor, 'registers')
def get_sensor_register_value(self, sensor, register):
return self._get_register_value(sensor, register)
def set_sensor_register_value(self, sensor, register, value):
return self._set_register_value(sensor, register, value)
# Access primitive related values
def get_primitives_list(self):
return [p.name for p in self.robot.primitives]
def get_running_primitives_list(self):
return [p.name for p in self.robot.active_primitives if hasattr(p, 'name')]
def start_primitive(self, primitive):
self._call_primitive_method(primitive, 'start')
def stop_primitive(self, primitive):
self._call_primitive_method(primitive, 'stop')
def pause_primitive(self, primitive):
self._call_primitive_method(primitive, 'pause')
def resume_primitive(self, primitive):
self._call_primitive_method(primitive, 'resume')
def get_primitive_properties_list(self, primitive):
return getattr(self.robot, primitive).properties
def get_primitive_property(self, primitive, property):
return self._get_register_value(primitive, property)
def set_primitive_property(self, primitive, property, value):
self._set_register_value(primitive, property, value)
def get_primitive_methods_list(self, primitive):
return getattr(self.robot, primitive).methods
def call_primitive_method(self, primitive, method, kwargs):
self._call_primitive_method(primitive, method, **kwargs)
def _set_register_value(self, object, register, value):
o = getattr(self.robot, object)
getattr(o, register) # does register exists ?
setattr(o, register, value)
def _get_register_value(self, object, register):
return attrgetter('{}.{}'.format(object, register))(self.robot)
def _call_primitive_method(self, primitive, method_name, *args, **kwargs):
p = getattr(self.robot, primitive)
f = getattr(p, method_name)
return f(*args, **kwargs)
# TODO (Theo) : change names with a dic instead of ugly format
def start_move_recorder(self, move_name, motors_name=None):
if not hasattr(self.robot, '_{}_recorder'.format(move_name)):
if motors_name is not None:
motors = [getattr(self.robot, m) for m in motors_name]
else:
motors = getattr(self.robot, 'motors')
recorder = MoveRecorder(self.robot, 50, motors)
self.robot.attach_primitive(recorder, '_{}_recorder'.format(move_name))
recorder.start()
else:
recorder = getattr(self.robot, '_{}_recorder'.format(move_name))
recorder.start()
def attach_move_recorder(self, move_name, motors_name):
motors = [getattr(self.robot, m) for m in motors_name]
recorder = MoveRecorder(self.robot, 50, motors)
self.robot.attach_primitive(recorder, '_{}_recorder'.format(move_name))
def get_move_recorder_motors(self, move_name):
try:
recorder = getattr(self.robot, '_{}_recorder'.format(move_name))
return [str(m.name) for m in recorder.tracked_motors]
except AttributeError:
return None
def get_move_recorder(self, move_name):
try:
recorder = getattr(self.robot, '_{}_recorder'.format(move_name))
move = recorder.move
return move.positions()
except AttributeError:
raise FileNotFoundError('I was not able to find _{}_recorder'.format(move_name))
def stop_move_recorder(self, move_name):
"""Allow more easily than stop_primitive() to save in a filename the recorded move"""
recorder = getattr(self.robot, '_{}_recorder'.format(move_name))
recorder.stop()
with open(self.moves_path.joinpath("{}.record".format(move_name)), 'w') as f:
recorder.move.save(f)
# Stop player if running : to discuss
# Recording a playing move can produce strange outputs, but could be a good feature
try:
player = getattr(self.robot, '_{}_player'.format(move_name))
if player.running:
player.stop()
except AttributeError:
pass
def start_move_player(self, move_name, speed=1.0, backwards=False):
"""Move player need to have a move file
<move_name.record> in the working directory to play it"""
# check if running
try:
player = getattr(self.robot, '_{}_player'.format(move_name))
if player.running:
return
except AttributeError:
pass
# if not running, override the play primitive
with open(self.moves_path.joinpath("{}.record".format(move_name))) as f:
loaded_move = Move.load(f)
player = MovePlayer(self.robot, loaded_move, play_speed=speed, backwards=backwards)
self.robot.attach_primitive(player, '_{}_player'.format(move_name))
player.start()
return player.duration()
def get_available_record_list(self):
"""Get list of json recorded movement files"""
return [f.stem for f in self.moves_path.glob('*.record') if f.is_file()]
def remove_move_record(self, move_name):
"""Remove the json recorded movement file"""
try:
os.remove(self.moves_path.joinpath("{}.record".format(move_name)))
return True
except FileNotFoundError:
return False
def getFrameFromCamera(self):
"""Gets and encodes the camera frame to .png format"""
_, img = cv2.imencode('.png', self.robot.camera.frame)
return img.tobytes()
def markers_list(self):
"""Gives the ids of all readable markers in front of the camera"""
detected_markers = self.robot.marker_detector.markers
return [m.id for m in detected_markers]
def detect_marker(self, marker):
"""Returns a boolean depending on whether the name of the qrcode given in parameter is visible by the camera"""
markers = {
'tetris': [112259237],
'caribou': [221052793],
'lapin': [44616414],
'rabbit': [44616414],
}
detected_markers_ids = self.markers_list()
return any([m in markers[marker] for m in detected_markers_ids])
# IK
def ik_endeffector(self, chain):
"""
Gives position & orientation of the end effector
:param chain: name of the IK chain
:return: tuple of strings for position & orientation ("x,y,z", "Rx.x,Rx.y,Rx.z")
"""
c = getattr(self.robot, chain)
position = ','.join(map(str, list(round(c.position, 4))))
orientation = ','.join(map(str, list(round(c.orientation, 4))))
return position, orientation
def ik_goto(self, chain, xyz, rot, duration, wait=False):
"""
goto a position defined by a xyz and/or an orientation
:param chain: name of the IK chain
:param xyz: cartesian coordinates (list of floats, in m)
:param rot: [Rx.x, Rx.y, Rx.z] (see https://www.brainvoyager.com/bv/doc/UsersGuide/CoordsAndTransforms/SpatialTransformationMatrices.html)
:param duration: duration of the movement (float, in s)
:param wait: do we wait the end of the move before giving the answer ? (boolean)
:return: Gives position & orientation of the end effector after the move
"""
c = getattr(self.robot, chain)
c.goto(xyz, rot, duration, wait)
return self.ik_endeffector(chain)
def ik_rpy(self, chain, roll, pitch, yaw):
"""
Gives the 3x3 affine rotation matrix corresponding the rpy values given.
:param chain: name of the IK chain
:param roll: float, -pi to pi
:param pitch: float, -pi to pi
:param yaw: float, -pi to pi
:return: 3x3 affine rotation matrix
"""
c = getattr(self.robot, chain)
return c.rpy_to_rotation_matrix(roll, pitch, yaw)

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