From 0aee7802af51b1c24626af082b9f88ba84bc10f7 Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Mon, 1 Jul 2019 01:55:04 +0200 Subject: [PATCH] update priority tasks --- pyrobolearn/priorities/CHANGELOG.txt | 9 + pyrobolearn/priorities/COPYRIGHT | 3 + pyrobolearn/priorities/LICENSE | 142 +++++ pyrobolearn/priorities/README.md | 56 -- pyrobolearn/priorities/README.rst | 68 +++ pyrobolearn/priorities/__init__.py | 9 +- .../constraints/{README.md => README.rst} | 10 +- .../priorities/constraints/__init__.py | 16 +- .../constraints/acceleration/__init__.py | 0 .../acceleration/dynamic_feasibility.py | 32 + .../priorities/constraints/constraint.py | 300 ++++++++-- .../constraints/dynamic_constraints.py | 1 + .../priorities/constraints/force/__init__.py | 0 .../priorities/constraints/force/contact.py | 37 ++ .../priorities/constraints/force/cop.py | 53 ++ .../priorities/constraints/force/friction.py | 79 +++ .../constraints/force/wrench_limits.py | 32 + .../priorities/constraints/force/zmp.py | 80 +++ .../priorities/constraints/torque/__init__.py | 0 .../constraints/torque/joint_limits.py | 32 + .../constraints/torque/torque_limits.py | 32 + .../constraints/velocity/__init__.py | 0 .../constraints/velocity/capture_point.py | 32 + .../velocity/cartesian_position.py | 32 + .../velocity/cartesian_velocity.py | 32 + .../constraints/velocity/com_velocity.py | 32 + .../constraints/velocity/convex_hull.py | 32 + .../constraints/velocity/dynamics.py | 32 + .../constraints/velocity/joint_limits.py | 64 ++ .../constraints/velocity/joint_velocity.py | 54 ++ .../velocity/self_collision_avoidance.py | 32 + .../constraints/velocity/velocity_limits.py | 55 ++ pyrobolearn/priorities/model.py | 111 ---- pyrobolearn/priorities/models/README.rst | 16 + pyrobolearn/priorities/models/__init__.py | 9 + pyrobolearn/priorities/models/model.py | 378 ++++++++++++ pyrobolearn/priorities/models/rbdl_model.py | 367 ++++++++++++ pyrobolearn/priorities/models/robot_model.py | 388 ++++++++++++ pyrobolearn/priorities/solvers/README.rst | 15 + pyrobolearn/priorities/solvers/__init__.py | 9 + .../priorities/solvers/nlp_task_solver.py | 64 ++ .../priorities/solvers/qp_task_solver.py | 60 ++ .../{solver.py => solvers/task_solver.py} | 42 +- pyrobolearn/priorities/tasks/README.md | 15 - pyrobolearn/priorities/tasks/README.rst | 114 ++++ pyrobolearn/priorities/tasks/__init__.py | 16 +- .../priorities/tasks/acceleration/__init__.py | 8 + .../tasks/acceleration/cartesian.py | 102 ++++ .../priorities/tasks/acceleration/com.py | 39 ++ .../priorities/tasks/acceleration/contact.py | 39 ++ .../priorities/tasks/acceleration/postural.py | 39 ++ .../priorities/tasks/force/__init__.py | 6 + pyrobolearn/priorities/tasks/force/com.py | 39 ++ .../priorities/tasks/force/floating_base.py | 39 ++ .../priorities/tasks/force/manipulability.py | 46 ++ pyrobolearn/priorities/tasks/force/wrench.py | 39 ++ pyrobolearn/priorities/tasks/task.py | 555 ++++++++++++++---- .../priorities/tasks/torque/__init__.py | 4 + .../torque/cartesian_impedance_control.py | 206 +++++++ .../tasks/torque/joint_impedance_control.py | 184 ++++++ .../priorities/tasks/velocity/__init__.py | 32 + .../tasks/velocity/angular_momentum.py | 150 +++++ .../priorities/tasks/velocity/cartesian.py | 170 ++++++ pyrobolearn/priorities/tasks/velocity/com.py | 150 +++++ .../priorities/tasks/velocity/contact.py | 106 ++++ pyrobolearn/priorities/tasks/velocity/gaze.py | 57 ++ .../priorities/tasks/velocity/interaction.py | 75 +++ .../tasks/velocity/linear_momentum.py | 150 +++++ .../tasks/velocity/manipulability.py | 62 ++ .../tasks/velocity/minimum_acceleration.py | 59 ++ .../tasks/velocity/minimum_effort.py | 48 ++ .../tasks/velocity/minimum_velocity.py | 53 ++ .../priorities/tasks/velocity/momentum.py | 146 +++++ .../priorities/tasks/velocity/postural.py | 156 +++++ .../priorities/tasks/velocity/pure_rolling.py | 64 ++ .../tasks/velocity/rigid_rotation.py | 48 ++ .../priorities/tasks/velocity/unicycle.py | 51 ++ 77 files changed, 5542 insertions(+), 372 deletions(-) create mode 100644 pyrobolearn/priorities/CHANGELOG.txt create mode 100644 pyrobolearn/priorities/COPYRIGHT create mode 100644 pyrobolearn/priorities/LICENSE delete mode 100644 pyrobolearn/priorities/README.md create mode 100644 pyrobolearn/priorities/README.rst rename pyrobolearn/priorities/constraints/{README.md => README.rst} (56%) create mode 100644 pyrobolearn/priorities/constraints/acceleration/__init__.py create mode 100644 pyrobolearn/priorities/constraints/acceleration/dynamic_feasibility.py create mode 100644 pyrobolearn/priorities/constraints/force/__init__.py create mode 100644 pyrobolearn/priorities/constraints/force/contact.py create mode 100644 pyrobolearn/priorities/constraints/force/cop.py create mode 100644 pyrobolearn/priorities/constraints/force/friction.py create mode 100644 pyrobolearn/priorities/constraints/force/wrench_limits.py create mode 100644 pyrobolearn/priorities/constraints/force/zmp.py create mode 100644 pyrobolearn/priorities/constraints/torque/__init__.py create mode 100644 pyrobolearn/priorities/constraints/torque/joint_limits.py create mode 100644 pyrobolearn/priorities/constraints/torque/torque_limits.py create mode 100644 pyrobolearn/priorities/constraints/velocity/__init__.py create mode 100644 pyrobolearn/priorities/constraints/velocity/capture_point.py create mode 100644 pyrobolearn/priorities/constraints/velocity/cartesian_position.py create mode 100644 pyrobolearn/priorities/constraints/velocity/cartesian_velocity.py create mode 100644 pyrobolearn/priorities/constraints/velocity/com_velocity.py create mode 100644 pyrobolearn/priorities/constraints/velocity/convex_hull.py create mode 100644 pyrobolearn/priorities/constraints/velocity/dynamics.py create mode 100644 pyrobolearn/priorities/constraints/velocity/joint_limits.py create mode 100644 pyrobolearn/priorities/constraints/velocity/joint_velocity.py create mode 100644 pyrobolearn/priorities/constraints/velocity/self_collision_avoidance.py create mode 100644 pyrobolearn/priorities/constraints/velocity/velocity_limits.py delete mode 100644 pyrobolearn/priorities/model.py create mode 100644 pyrobolearn/priorities/models/README.rst create mode 100644 pyrobolearn/priorities/models/__init__.py create mode 100644 pyrobolearn/priorities/models/model.py create mode 100644 pyrobolearn/priorities/models/rbdl_model.py create mode 100644 pyrobolearn/priorities/models/robot_model.py create mode 100644 pyrobolearn/priorities/solvers/README.rst create mode 100644 pyrobolearn/priorities/solvers/__init__.py create mode 100644 pyrobolearn/priorities/solvers/nlp_task_solver.py create mode 100644 pyrobolearn/priorities/solvers/qp_task_solver.py rename pyrobolearn/priorities/{solver.py => solvers/task_solver.py} (53%) delete mode 100644 pyrobolearn/priorities/tasks/README.md create mode 100644 pyrobolearn/priorities/tasks/README.rst create mode 100644 pyrobolearn/priorities/tasks/acceleration/__init__.py create mode 100644 pyrobolearn/priorities/tasks/acceleration/cartesian.py create mode 100644 pyrobolearn/priorities/tasks/acceleration/com.py create mode 100644 pyrobolearn/priorities/tasks/acceleration/contact.py create mode 100644 pyrobolearn/priorities/tasks/acceleration/postural.py create mode 100644 pyrobolearn/priorities/tasks/force/__init__.py create mode 100644 pyrobolearn/priorities/tasks/force/com.py create mode 100644 pyrobolearn/priorities/tasks/force/floating_base.py create mode 100644 pyrobolearn/priorities/tasks/force/manipulability.py create mode 100644 pyrobolearn/priorities/tasks/force/wrench.py create mode 100644 pyrobolearn/priorities/tasks/torque/__init__.py create mode 100644 pyrobolearn/priorities/tasks/torque/cartesian_impedance_control.py create mode 100644 pyrobolearn/priorities/tasks/torque/joint_impedance_control.py create mode 100644 pyrobolearn/priorities/tasks/velocity/__init__.py create mode 100644 pyrobolearn/priorities/tasks/velocity/angular_momentum.py create mode 100644 pyrobolearn/priorities/tasks/velocity/cartesian.py create mode 100644 pyrobolearn/priorities/tasks/velocity/com.py create mode 100644 pyrobolearn/priorities/tasks/velocity/contact.py create mode 100644 pyrobolearn/priorities/tasks/velocity/gaze.py create mode 100644 pyrobolearn/priorities/tasks/velocity/interaction.py create mode 100644 pyrobolearn/priorities/tasks/velocity/linear_momentum.py create mode 100644 pyrobolearn/priorities/tasks/velocity/manipulability.py create mode 100644 pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py create mode 100644 pyrobolearn/priorities/tasks/velocity/minimum_effort.py create mode 100644 pyrobolearn/priorities/tasks/velocity/minimum_velocity.py create mode 100644 pyrobolearn/priorities/tasks/velocity/momentum.py create mode 100644 pyrobolearn/priorities/tasks/velocity/postural.py create mode 100644 pyrobolearn/priorities/tasks/velocity/pure_rolling.py create mode 100644 pyrobolearn/priorities/tasks/velocity/rigid_rotation.py create mode 100644 pyrobolearn/priorities/tasks/velocity/unicycle.py diff --git a/pyrobolearn/priorities/CHANGELOG.txt b/pyrobolearn/priorities/CHANGELOG.txt new file mode 100644 index 0000000..fd456ec --- /dev/null +++ b/pyrobolearn/priorities/CHANGELOG.txt @@ -0,0 +1,9 @@ + +26/06/2019 + +- Translated most C++ code from the `OpenSoT` library to Python +- made it a standalone library (does not depend on other libraries (or middleware layers) except numpy) +- Add more complete documentation for each class and method (this was seriously lacking in the original OpenSoT) +- Improve methods and made them more Pythonic +- Add few extra constraints and tasks (based on Songyan Xin's work) +- Integration with the PyRoboLearn framework through the robot model diff --git a/pyrobolearn/priorities/COPYRIGHT b/pyrobolearn/priorities/COPYRIGHT new file mode 100644 index 0000000..7ac4a20 --- /dev/null +++ b/pyrobolearn/priorities/COPYRIGHT @@ -0,0 +1,3 @@ +Copyright (c) 2014, Alessio Rocchi , Enrico Mingo Hoffman , Arturo Laurenzi , Cheng Fang +Translated from C++ to Python by Brian Delhaisse (see CHANGELOG file) +C++ Forked Version available at https://github.com/yangweiyou/OpenSoT diff --git a/pyrobolearn/priorities/LICENSE b/pyrobolearn/priorities/LICENSE new file mode 100644 index 0000000..4114b2b --- /dev/null +++ b/pyrobolearn/priorities/LICENSE @@ -0,0 +1,142 @@ +GNU LESSER GENERAL PUBLIC LICENSE + +Version 2.1, February 1999 + +Copyright (C) 1991, 1999 Free Software Foundation, Inc. +51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA +Everyone is permitted to copy and distribute verbatim copies +of this license document, but changing it is not allowed. + +[This is the first released version of the Lesser GPL. It also counts + as the successor of the GNU Library Public License, version 2, hence + the version number 2.1.] + +Preamble + +The licenses for most software are designed to take away your freedom to share and change it. By contrast, the GNU General Public Licenses are intended to guarantee your freedom to share and change free software--to make sure the software is free for all its users. + +This license, the Lesser General Public License, applies to some specially designated software packages--typically libraries--of the Free Software Foundation and other authors who decide to use it. You can use it too, but we suggest you first think carefully about whether this license or the ordinary General Public License is the better strategy to use in any particular case, based on the explanations below. + +When we speak of free software, we are referring to freedom of use, 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 this service if you wish); that you receive source code or can get it if you want it; that you can change the software and use pieces of it in new free programs; and that you are informed that you can do these things. + +To protect your rights, we need to make restrictions that forbid distributors to deny you these rights or to ask you to surrender these rights. These restrictions translate to certain responsibilities for you if you distribute copies of the library or if you modify it. + +For example, if you distribute copies of the library, whether gratis or for a fee, you must give the recipients all the rights that we gave you. You must make sure that they, too, receive or can get the source code. If you link other code with the library, you must provide complete object files to the recipients, so that they can relink them with the library after making changes to the library and recompiling it. And you must show them these terms so they know their rights. + +We protect your rights with a two-step method: (1) we copyright the library, and (2) we offer you this license, which gives you legal permission to copy, distribute and/or modify the library. + +To protect each distributor, we want to make it very clear that there is no warranty for the free library. Also, if the library is modified by someone else and passed on, the recipients should know that what they have is not the original version, so that the original author's reputation will not be affected by problems that might be introduced by others. + +Finally, software patents pose a constant threat to the existence of any free program. We wish to make sure that a company cannot effectively restrict the users of a free program by obtaining a restrictive license from a patent holder. Therefore, we insist that any patent license obtained for a version of the library must be consistent with the full freedom of use specified in this license. + +Most GNU software, including some libraries, is covered by the ordinary GNU General Public License. This license, the GNU Lesser General Public License, applies to certain designated libraries, and is quite different from the ordinary General Public License. We use this license for certain libraries in order to permit linking those libraries into non-free programs. + +When a program is linked with a library, whether statically or using a shared library, the combination of the two is legally speaking a combined work, a derivative of the original library. The ordinary General Public License therefore permits such linking only if the entire combination fits its criteria of freedom. The Lesser General Public License permits more lax criteria for linking other code with the library. + +We call this license the "Lesser" General Public License because it does Less to protect the user's freedom than the ordinary General Public License. It also provides other free software developers Less of an advantage over competing non-free programs. These disadvantages are the reason we use the ordinary General Public License for many libraries. However, the Lesser license provides advantages in certain special circumstances. + +For example, on rare occasions, there may be a special need to encourage the widest possible use of a certain library, so that it becomes a de-facto standard. To achieve this, non-free programs must be allowed to use the library. A more frequent case is that a free library does the same job as widely used non-free libraries. In this case, there is little to gain by limiting the free library to free software only, so we use the Lesser General Public License. + +In other cases, permission to use a particular library in non-free programs enables a greater number of people to use a large body of free software. For example, permission to use the GNU C Library in non-free programs enables many more people to use the whole GNU operating system, as well as its variant, the GNU/Linux operating system. + +Although the Lesser General Public License is Less protective of the users' freedom, it does ensure that the user of a program that is linked with the Library has the freedom and the wherewithal to run that program using a modified version of the Library. + +The precise terms and conditions for copying, distribution and modification follow. Pay close attention to the difference between a "work based on the library" and a "work that uses the library". The former contains code derived from the library, whereas the latter must be combined with the library in order to run. +TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + +0. This License Agreement applies to any software library or other program which contains a notice placed by the copyright holder or other authorized party saying it may be distributed under the terms of this Lesser General Public License (also called "this License"). Each licensee is addressed as "you". + +A "library" means a collection of software functions and/or data prepared so as to be conveniently linked with application programs (which use some of those functions and data) to form executables. + +The "Library", below, refers to any such software library or work which has been distributed under these terms. A "work based on the Library" means either the Library or any derivative work under copyright law: that is to say, a work containing the Library or a portion of it, either verbatim or with modifications and/or translated straightforwardly into another language. (Hereinafter, translation is included without limitation in the term "modification".) + +"Source code" for a work means the preferred form of the work for making modifications to it. For a library, complete source code means all the source code for all modules it contains, plus any associated interface definition files, plus the scripts used to control compilation and installation of the library. + +Activities other than copying, distribution and modification are not covered by this License; they are outside its scope. The act of running a program using the Library is not restricted, and output from such a program is covered only if its contents constitute a work based on the Library (independent of the use of the Library in a tool for writing it). Whether that is true depends on what the Library does and what the program that uses the Library does. + +1. You may copy and distribute verbatim copies of the Library's complete source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice and disclaimer of warranty; keep intact all the notices that refer to this License and to the absence of any warranty; and distribute a copy of this License along with the Library. + +You may charge a fee for the physical act of transferring a copy, and you may at your option offer warranty protection in exchange for a fee. + +2. You may modify your copy or copies of the Library or any portion of it, thus forming a work based on the Library, and copy and distribute such modifications or work under the terms of Section 1 above, provided that you also meet all of these conditions: + + a) The modified work must itself be a software library. + b) You must cause the files modified to carry prominent notices stating that you changed the files and the date of any change. + c) You must cause the whole of the work to be licensed at no charge to all third parties under the terms of this License. + d) If a facility in the modified Library refers to a function or a table of data to be supplied by an application program that uses the facility, other than as an argument passed when the facility is invoked, then you must make a good faith effort to ensure that, in the event an application does not supply such function or table, the facility still operates, and performs whatever part of its purpose remains meaningful. + + (For example, a function in a library to compute square roots has a purpose that is entirely well-defined independent of the application. Therefore, Subsection 2d requires that any application-supplied function or table used by this function must be optional: if the application does not supply it, the square root function must still compute square roots.) + +These requirements apply to the modified work as a whole. If identifiable sections of that work are not derived from the Library, and can be reasonably considered independent and separate works in themselves, then this License, and its terms, do not apply to those sections when you distribute them as separate works. But when you distribute the same sections as part of a whole which is a work based on the Library, the distribution of the whole must be on the terms of this License, whose permissions for other licensees extend to the entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest your rights to work written entirely by you; rather, the intent is to exercise the right to control the distribution of derivative or collective works based on the Library. + +In addition, mere aggregation of another work not based on the Library with the Library (or with a work based on the Library) on a volume of a storage or distribution medium does not bring the other work under the scope of this License. + +3. You may opt to apply the terms of the ordinary GNU General Public License instead of this License to a given copy of the Library. To do this, you must alter all the notices that refer to this License, so that they refer to the ordinary GNU General Public License, version 2, instead of to this License. (If a newer version than version 2 of the ordinary GNU General Public License has appeared, then you can specify that version instead if you wish.) Do not make any other change in these notices. + +Once this change is made in a given copy, it is irreversible for that copy, so the ordinary GNU General Public License applies to all subsequent copies and derivative works made from that copy. + +This option is useful when you wish to copy part of the code of the Library into a program that is not a library. + +4. You may copy and distribute the Library (or a portion or derivative of it, under Section 2) in object code or executable form under the terms of Sections 1 and 2 above provided that you accompany it with the complete corresponding machine-readable source code, which must be distributed under the terms of Sections 1 and 2 above on a medium customarily used for software interchange. + +If distribution of object code is made by offering access to copy from a designated place, then offering equivalent access to copy the source code from the same place satisfies the requirement to distribute the source code, even though third parties are not compelled to copy the source along with the object code. + +5. A program that contains no derivative of any portion of the Library, but is designed to work with the Library by being compiled or linked with it, is called a "work that uses the Library". Such a work, in isolation, is not a derivative work of the Library, and therefore falls outside the scope of this License. + +However, linking a "work that uses the Library" with the Library creates an executable that is a derivative of the Library (because it contains portions of the Library), rather than a "work that uses the library". The executable is therefore covered by this License. Section 6 states terms for distribution of such executables. + +When a "work that uses the Library" uses material from a header file that is part of the Library, the object code for the work may be a derivative work of the Library even though the source code is not. Whether this is true is especially significant if the work can be linked without the Library, or if the work is itself a library. The threshold for this to be true is not precisely defined by law. + +If such an object file uses only numerical parameters, data structure layouts and accessors, and small macros and small inline functions (ten lines or less in length), then the use of the object file is unrestricted, regardless of whether it is legally a derivative work. (Executables containing this object code plus portions of the Library will still fall under Section 6.) + +Otherwise, if the work is a derivative of the Library, you may distribute the object code for the work under the terms of Section 6. Any executables containing that work also fall under Section 6, whether or not they are linked directly with the Library itself. + +6. As an exception to the Sections above, you may also combine or link a "work that uses the Library" with the Library to produce a work containing portions of the Library, and distribute that work under terms of your choice, provided that the terms permit modification of the work for the customer's own use and reverse engineering for debugging such modifications. + +You must give prominent notice with each copy of the work that the Library is used in it and that the Library and its use are covered by this License. You must supply a copy of this License. If the work during execution displays copyright notices, you must include the copyright notice for the Library among them, as well as a reference directing the user to the copy of this License. Also, you must do one of these things: + + a) Accompany the work with the complete corresponding machine-readable source code for the Library including whatever changes were used in the work (which must be distributed under Sections 1 and 2 above); and, if the work is an executable linked with the Library, with the complete machine-readable "work that uses the Library", as object code and/or source code, so that the user can modify the Library and then relink to produce a modified executable containing the modified Library. (It is understood that the user who changes the contents of definitions files in the Library will not necessarily be able to recompile the application to use the modified definitions.) + b) Use a suitable shared library mechanism for linking with the Library. A suitable mechanism is one that (1) uses at run time a copy of the library already present on the user's computer system, rather than copying library functions into the executable, and (2) will operate properly with a modified version of the library, if the user installs one, as long as the modified version is interface-compatible with the version that the work was made with. + c) Accompany the work with a written offer, valid for at least three years, to give the same user the materials specified in Subsection 6a, above, for a charge no more than the cost of performing this distribution. + d) If distribution of the work is made by offering access to copy from a designated place, offer equivalent access to copy the above specified materials from the same place. + e) Verify that the user has already received a copy of these materials or that you have already sent this user a copy. + +For an executable, the required form of the "work that uses the Library" must include any data and utility programs needed for reproducing the executable from it. However, as a special exception, the materials to be distributed need not include anything that is normally distributed (in either source or binary form) with the major components (compiler, kernel, and so on) of the operating system on which the executable runs, unless that component itself accompanies the executable. + +It may happen that this requirement contradicts the license restrictions of other proprietary libraries that do not normally accompany the operating system. Such a contradiction means you cannot use both them and the Library together in an executable that you distribute. + +7. You may place library facilities that are a work based on the Library side-by-side in a single library together with other library facilities not covered by this License, and distribute such a combined library, provided that the separate distribution of the work based on the Library and of the other library facilities is otherwise permitted, and provided that you do these two things: + + a) Accompany the combined library with a copy of the same work based on the Library, uncombined with any other library facilities. This must be distributed under the terms of the Sections above. + b) Give prominent notice with the combined library of the fact that part of it is a work based on the Library, and explaining where to find the accompanying uncombined form of the same work. + +8. You may not copy, modify, sublicense, link with, or distribute the Library except as expressly provided under this License. Any attempt otherwise to copy, modify, sublicense, link with, or distribute the Library is void, and will automatically terminate your rights under this License. However, parties who have received copies, or rights, from you under this License will not have their licenses terminated so long as such parties remain in full compliance. + +9. You are not required to accept this License, since you have not signed it. However, nothing else grants you permission to modify or distribute the Library or its derivative works. These actions are prohibited by law if you do not accept this License. Therefore, by modifying or distributing the Library (or any work based on the Library), you indicate your acceptance of this License to do so, and all its terms and conditions for copying, distributing or modifying the Library or works based on it. + +10. Each time you redistribute the Library (or any work based on the Library), the recipient automatically receives a license from the original licensor to copy, distribute, link with or modify the Library subject to these terms and conditions. You may not impose any further restrictions on the recipients' exercise of the rights granted herein. You are not responsible for enforcing compliance by third parties with this License. + +11. If, as a consequence of a court judgment or allegation of patent infringement or for any other reason (not limited to patent issues), 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 distribute so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not distribute the Library at all. For example, if a patent license would not permit royalty-free redistribution of the Library by all those who receive copies directly or indirectly through you, then the only way you could satisfy both it and this License would be to refrain entirely from distribution of the Library. + +If any portion of this section is held invalid or unenforceable under any particular circumstance, the balance of the section is intended to apply, and the section as a whole is intended to apply in other circumstances. + +It is not the purpose of this section to induce you to infringe any patents or other property right claims or to contest validity of any such claims; this section has the sole purpose of protecting the integrity of the free software distribution system which is implemented by public license practices. Many people have made generous contributions to the wide range of software distributed through that system in reliance on consistent application of that system; it is up to the author/donor to decide if he or she is willing to distribute software through any other system and a licensee cannot impose that choice. + +This section is intended to make thoroughly clear what is believed to be a consequence of the rest of this License. + +12. If the distribution and/or use of the Library is restricted in certain countries either by patents or by copyrighted interfaces, the original copyright holder who places the Library under this License may add an explicit geographical distribution limitation excluding those countries, so that distribution is permitted only in or among countries not thus excluded. In such case, this License incorporates the limitation as if written in the body of this License. + +13. The Free Software Foundation may publish revised and/or new versions of the Lesser 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 Library specifies a version number of this License which applies to it and "any later version", you have the option of following the terms and conditions either of that version or of any later version published by the Free Software Foundation. If the Library does not specify a license version number, you may choose any version ever published by the Free Software Foundation. + +14. If you wish to incorporate parts of the Library into other free programs whose distribution conditions are incompatible with these, write to the author to ask for permission. For software which is copyrighted by the Free Software Foundation, write to the Free Software Foundation; we sometimes make exceptions for this. Our decision will be guided by the two goals of preserving the free status of all derivatives of our free software and of promoting the sharing and reuse of software generally. + +NO WARRANTY + +15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE LIBRARY "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 LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + +16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR REDISTRIBUTE THE LIBRARY 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 LIBRARY (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 LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. +END OF TERMS AND CONDITIONS diff --git a/pyrobolearn/priorities/README.md b/pyrobolearn/priorities/README.md deleted file mode 100644 index bf4beba..0000000 --- a/pyrobolearn/priorities/README.md +++ /dev/null @@ -1,56 +0,0 @@ -## Priority Tasks - -In this folder, you will find the code for priority "tasks". The "tasks" defined here are different from the tasks -defined in the `pyrobolearn/tasks` folder which defines robot learning tasks. The tasks defined here can be more seen -as "constraints"; for instance, the constraint for the robot to maintain its balance (i.e. have its center of mass -above the support polygon), the constraint for the robot to have a certain pose, the constraint for the robot's -end-effectors to track a certain trajectory, the constraint for the robot to not have collisions between its links, -the constraint for the robot to respect the equation of motions, etc. In the robotics community field, these are known -as "tasks" and concepts around them such as the stack of tasks [2] have been defined. In *pyrobolearn*, we keep a -more abstract definition of a task, which is also probably more related to what people have in mind when talking -about a robot performing a certain task. - -Most of these "tasks" are represented as a constrained optimization problem, where the "task" consists to minimize -a certain objective function while respecting certain equality and inequality constraints. This is the reason why -they are not called "constraints" to avoid the confusion with the (inequality and equality) constraints defined in -the optimization problem. Most of the time, they are formulated as quadratic programming (QP) optimization problem [1]. -Priority tasks are divided between kinematic and dynamic tasks, where the former only takes into account position and -velocity information, while the latter also include dynamic information (forces and torques applied on the various -bodies). The variables that are thus optimized by the optimization problem depends on the type of problem (kinematic -or dynamic) we are dealing with. In the case of a kinematic task, the variables are often the joint (or end-effector) -positions and/or velocities, while in the dynamic case, the variables are the joint accelerations and the (reaction) -forces applied on the robot. - -Priorities can be divided into two categories: soft and hard priorities. -* Soft priorities: each objective function is weigthed by an importance weight where higher weights mean that we give -more importance to the corresponding objective function. For instance, we might have a humanoid robot with two arms -where each arm has to follow a specific trajectory and where we give the same importance to both "tasks". Soft -priorities use task augmentation. -* hard priorities: the most important constrained optimization problem is first solved, and then the next most -important one is solved with an additional (optimization) constraint that the solution has to be in the solution space -of the previous one. For instance, it is more important for a humanoid robot to maintain its balance than to follow -perfectly a trajectory with its end-effector. This way of putting "tasks" on top of each other is known as the stack -of tasks in the robotics community [2]. Hard priorities exploit the null-space of higher priority tasks. - -Soft and hard priorities can be mixed together as done in the following C++ framework [3]. - -The code presented here (and the architecture) is partially inspired by [3, 4] (the papers and slides). Compared to -this framework, we write it in Python using popular optimization libraries (that are usually written in C/C++ and -provide Python wrappers), we decouple it from other frameworks/middlewares (such as superbuild, XBotControl, -ROS/Yarp, etc.), and make it free and open-source (currently, the original code seems to be on a private repo). - -In what follows, to avoid confusion with the vocabulary used in the robotics community, we will keep the notions of -"tasks", (optimization) "constraints", and "solvers". The solvers can be found in the `pyrobolearn/optimizers` folder. - - -## References - -1. Quadratic Programming (Wikipedia): https://en.wikipedia.org/wiki/Quadratic_programming -2. "A Versatile Generalized Inverted Kinematics Implementationfor Collaborative Working Humanoid Robots: The Stack of -Tasks" ([code](https://stack-of-tasks.github.io/)), Mansard et al., 2009 -3. "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" ( - [code](https://opensot.wixsite.com/opensot), - [slides](https://docs.google.com/presentation/d/1kwJsAnVi_3ADtqFSTP8wq3JOGLcvDV_ypcEEjPHnCEA), - [tutorial video](https://www.youtube.com/watch?v=yFon-ZDdSyg), [old code](https://github.com/songcheng/OpenSoT)), - Rocchi et al., 2015 -4. "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 diff --git a/pyrobolearn/priorities/README.rst b/pyrobolearn/priorities/README.rst new file mode 100644 index 0000000..6e4154d --- /dev/null +++ b/pyrobolearn/priorities/README.rst @@ -0,0 +1,68 @@ +Priority Tasks +============== + +In this folder, you will find the code for priority "tasks". The "tasks" defined here are different from the tasks +defined in the ``pyrobolearn/tasks`` folder which defines robot learning tasks. The tasks defined here can be more seen +as "constraints"; for instance, the constraint for the robot to maintain its balance (i.e. have its center of mass +above the support polygon), the constraint for the robot to have a certain pose, the constraint for the robot's +end-effectors to track a certain trajectory, the constraint for the robot to not have collisions between its links, +the constraint for the robot to respect the equation of motions, etc. In the robotics community field, these are known +as "tasks" and concepts around them such as the stack of tasks [2]_ have been defined. In *pyrobolearn*, we keep a +more abstract definition of a task, which is also probably more related to what people have in mind when talking +about a robot performing a certain task. + +Most of these "tasks" are represented as a constrained optimization problem, where the "task" consists to minimize +a certain objective function while respecting certain equality and inequality constraints. This is the reason why +they are not called "constraints" to avoid the confusion with the (inequality and equality) constraints defined in +the optimization problem. Most of the time, they are formulated as quadratic programming (QP) optimization problem [1]_. +Priority tasks are divided between kinematic and dynamic tasks, where the former only takes into account position and +velocity information, while the latter also include dynamic information (forces and torques applied on the various +bodies). The variables that are thus optimized by the optimization problem depends on the type of problem (kinematic +or dynamic) we are dealing with. In the case of a kinematic task, the variables are often the joint (or end-effector) +positions and/or velocities, while in the dynamic case, the variables are the joint accelerations and the (reaction) +forces applied on the robot. + +Priorities can be divided into two categories: + +- **Soft** priorities: each objective function is weighted by an importance weight where higher weights mean that we + give more importance to the corresponding objective function. For instance, we might have a humanoid robot with two + arms where each arm has to follow a specific trajectory and where we give the same importance to both "tasks". Soft + priorities use task augmentation. +- **Hard** priorities: the most important constrained optimization problem is first solved, and then the next most + important one is solved with an additional (optimization) constraint that the solution has to be in the solution + space of the previous one. For instance, it is more important for a humanoid robot to maintain its balance than to + follow perfectly a trajectory with its end-effector. This way of putting "tasks" on top of each other is known as + the stack of tasks in the robotics community [2]_. Hard priorities exploit the null-space of higher priority tasks. + +Soft and hard priorities can be mixed together as done in the following C++ framework [3]_. + +The code presented here (and the architecture) is mostly inspired by [3]_, [4]_ (the papers and slides). Compared to +this framework, we write it in Python using popular optimization libraries (that are usually written in C/C++ and +provide Python wrappers), we decouple it from other frameworks/middlewares (such as superbuild, XBotControl, +ROS/YARP, etc.), integrate it with PRL, and make it free and open-source. + +In what follows, to avoid confusion with the vocabulary used in the robotics community, we will keep the notions of +"tasks", (optimization) "constraints", and "solvers". The solvers can be found in the ``pyrobolearn/optimizers`` folder. + + +References +---------- + +.. [1] Quadratic Programming `(Wikipedia) `_ + +.. [2] "A Versatile Generalized Inverted Kinematics Implementation for Collaborative Working Humanoid Robots: The Stack of + Tasks" (`code `_), Mansard et al., 2009 + +.. [3] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" ( + `code `_, + `slides `_, + `tutorial video `_, `old code `_, + LGPLv2), Rocchi et al., 2015 + +.. [4] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 + + +TODO +~~~~ + +- [ ] create independent ``pyopensot`` package once it is over. diff --git a/pyrobolearn/priorities/__init__.py b/pyrobolearn/priorities/__init__.py index ecdff7b..6e42b99 100644 --- a/pyrobolearn/priorities/__init__.py +++ b/pyrobolearn/priorities/__init__.py @@ -1,12 +1,13 @@ # import model interface -from .model import ModelInterface +from . import models # import constraints -from .constraints import * +from . import constraints # import tasks -from .tasks import * +from . import tasks # import solvers -from .solver import * +from . import solvers + diff --git a/pyrobolearn/priorities/constraints/README.md b/pyrobolearn/priorities/constraints/README.rst similarity index 56% rename from pyrobolearn/priorities/constraints/README.md rename to pyrobolearn/priorities/constraints/README.rst index 88ede2e..17032d4 100644 --- a/pyrobolearn/priorities/constraints/README.md +++ b/pyrobolearn/priorities/constraints/README.rst @@ -1,4 +1,5 @@ -## Constraints +Constraints +=========== In this folder, we define the most common inequality and equality optimization constraints used in robotics for priority tasks. Several of them were provided in [1]. @@ -6,10 +7,7 @@ priority tasks. Several of them were provided in [1]. Constraints include joint limits, joint velocity limits, collision avoidance, and others. References: -1. "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" ( - [code](https://opensot.wixsite.com/opensot), - [slides](https://docs.google.com/presentation/d/1kwJsAnVi_3ADtqFSTP8wq3JOGLcvDV_ypcEEjPHnCEA), - [tutorial video](https://www.youtube.com/watch?v=yFon-ZDdSyg), - [old code](https://github.com/songcheng/OpenSoT)), Rocchi et al., 2015 + +1. "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" (`code `_, `slides `_, `tutorial video `_, `old code `_, LGPLv2), Rocchi et al., 2015 2. "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 diff --git a/pyrobolearn/priorities/constraints/__init__.py b/pyrobolearn/priorities/constraints/__init__.py index 0cf73e7..e924b3b 100644 --- a/pyrobolearn/priorities/constraints/__init__.py +++ b/pyrobolearn/priorities/constraints/__init__.py @@ -2,8 +2,20 @@ # import constraint from .constraint import * +# import velocity constraints +from . import velocity + +# import acceleration constraints +from . import acceleration + +# import torque constraints +from . import torque + +# import force constraints +from . import force + # import kinematic constraints -from .kinematic_constraints import * +# from .kinematic_constraints import * # import dynamic constraints -from .dynamic_constraints import * +# from .dynamic_constraints import * diff --git a/pyrobolearn/priorities/constraints/acceleration/__init__.py b/pyrobolearn/priorities/constraints/acceleration/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/priorities/constraints/acceleration/dynamic_feasibility.py b/pyrobolearn/priorities/constraints/acceleration/dynamic_feasibility.py new file mode 100644 index 0000000..e0d7eea --- /dev/null +++ b/pyrobolearn/priorities/constraints/acceleration/dynamic_feasibility.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python +r"""Provide the dynamic feasibility constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import Constraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Arturo Laurenzi (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class DynamicFeasibilityConstraint(Constraint): + r"""Dynamic Feasibility Constraint + + """ + + def __init__(self, model): + super(DynamicFeasibilityConstraint, self).__init__(model) diff --git a/pyrobolearn/priorities/constraints/constraint.py b/pyrobolearn/priorities/constraints/constraint.py index e6d2369..7a4aa2a 100644 --- a/pyrobolearn/priorities/constraints/constraint.py +++ b/pyrobolearn/priorities/constraints/constraint.py @@ -3,74 +3,89 @@ r"""Provide the various constraints and bounds used in QP. Provide the various optimization constraints (:math:`G, h, F, c` in the upcoming formulation) used in QP. -A quadratic program (QP) is written in standard form [1] as: +A quadratic program (QP) is written in standard form [1]_ as: .. math:: - x^* &= \arg \min_x \frac{1}{2} x^T Q x + p^T x \\ \text{subj. to} - & Gx \leq h \\ - & Fx = c + x^* =& \arg \min_x \; \frac{1}{2} x^T Q x + p^T x \\ + & \text{subj. to } \; \begin{array}{c} Gx \leq h \\ Fx = c \end{array} + where :math:`x` is the vector being optimized (in robotics, it can be joint positions, velocities, torques, ...), "the matrix :math:`Q` and vector :math:`p` are used to define any quadratic objective function of these variables, while the matrix-vector couples :math:`(G,h)` and :math:`(F,c)` respectively define inequality and equality -constraints" [1]. Inequality constraints can include the lower bounds and upper bounds of :math`x` by setting -:math:`G` to be the identity matrix or minus this one, and :math:`h` to be the upper or lower bounds. +constraints" [1]_. Inequality constraints can include the lower bounds and upper bounds of :math:`x` by setting +:math:`G` to be the identity matrix or minus this one, and :math:`h` to be the upper or minus the lower bounds. -For instance, the quadratic objective function :math:`||Ax - b||^2_{W}` (where :math:`W` is a weight matrix) is given -in the standard form as: +For instance, the quadratic objective function :math:`||Ax - b||_{W}^2` (where :math:`W` is a symmetric weight matrix) +is given in the standard form as: -.. math:: ||Ax - b||^2_{W} = (Ax - b)^\top W (Ax - b) = x^\top A^\top W A x - 2 b^\top W A x + b^\top W b +.. math:: ||Ax - b||_{W}^2 = (Ax - b)^\top W (Ax - b) = x^\top A^\top W A x - 2 b^\top W A x + b^\top W b where the last term :math:`b^\top W b` can be removed as it does not depend on the variables we are optimizing (i.e. -:math:`x`). We thus have :math:`Q = A^\top W A` a symmetric matrix and :math:`p = -2 b^\top W A`. +:math:`x`). We thus have :math:`Q = A^\top W A` a symmetric matrix and :math:`p = -2 A^\top W b`. -Many control problems in robotics can be formulated as a quadratic programming problem. +Note that if we had instead :math:`||Ax - b||_{W}^2 + c^\top x`, this could be rewritten as: -For instance, let's assume that we want to optimize the joint velocities :math:`\dot{q}` given the end-effector's -desired position and velocity in task space. We can define the quadratic problem as: +.. math:: ||Ax - b||_{W}^2 + c^\top x = x^\top A^\top W A x - (2 b^\top W A - c^\top) x + b^\top W b, -.. math:: || J(q) \dot{q} - \dot{x} ) ||^2 +giving :math:`Q = A^\top W A` and :math:`p = (c - 2 A^\top W b)`. -where using a PD reference, :math:`\dot{x} = \dot{x}_d + K (x_d - x)`, where :math:`x_d` and :math:`x` are the desired -and current end-effector's position respectively, and :math:`\dot{x}_d` is the desired velocity. +Many control problems in robotics can be formulated as a quadratic programming problem. For instance, let's assume +that we want to optimize the joint velocities :math:`\dot{q}` given the end-effector's desired position and velocity +in task space. We can define the quadratic problem as: + +.. math:: || J(q) \dot{q} - v_c ||^2 + +where :math:`v_c = K_p (x_d - x) + K_d (v_d - \dot{x})` (using PD control), with :math:`x_d` and :math:`x` the desired +and current end-effector's position respectively, and :math:`v_d` is the desired velocity. The solution to this +task (i.e. optimization problem) is the same solution given by `inverse kinematics`. Now, you can even obtain the +damped least squares inverse kinematics by adding a soft task such that +:math:`||J(q)\dot{q} - v_c||^2 + ||q||^2` is optimized (note that :math:`||q||^2 = ||A q - b||^2`, where :math:`A=I` is +the identity matrix and :math:`b=0` is the zero/null vector). -* Soft priority tasks: with soft-priority tasks, the quadratic programming problem being minimized for n such tasks -is given by: +- **Soft** priority tasks: with soft-priority tasks, the quadratic programming problem being minimized for :math:`n` + such tasks is given by: -.. math:: + .. math:: - x^* &= \arg \min_x ||A_1 x - b_1||^2_{W_1} + ||A_2 x - b_2 ||^2_{W_2} + ... + ||A_n x - b_n ||^2_{W_n} \\ - \text{subj. to} & Gx \leq h \\ - & Fx = c + \begin{array}{c} + x^* = \arg \min_x ||A_1 x - b_1||_{W_1}^2 + ||A_2 x - b_2 ||_{W_2}^2 + ... + ||A_n x - b_n ||_{W_n}^2 \\ + \text{subj. to } \; \begin{array}{c} Gx \leq h \\ Fx = c \end{array} + \end{array} -Often, the weight matrices :math:`W_i` are just scalars :math:`w_i`. This problem can notably be solved by stacking -the :math:`A_i` one of top of another, and stacking the :math:`b_i` and :math:`W_i` in the same manner, and solving -:math:`||A x - b||^2_{W}` This is known as the augmented task. When the matrices :math:`A` are Jacobians this is known -as the augmented Jacobian (which can sometimes be ill-conditioned). + Often, the weight PSD matrices :math:`W_i` are just positive scalars :math:`w_i`. This problem can notably be solved + by stacking the :math:`A_i` one of top of another, and stacking the :math:`b_i` and :math:`W_i` in the same manner, + and solving :math:`||A x - b||_{W}^2`. This is known as the augmented task. When the matrices :math:`A_i` are + Jacobians this is known as the augmented Jacobian (which can sometimes be ill-conditioned). -* Hard priority tasks: with hard-priority tasks, the quadratic programming problem for n tasks is defined in a -sequential manner, where the first most important task will be first optimized, and then the subsequent tasks will be -optimized one after the other. Thus, the first task to be optimized is given by: +- **Hard** priority tasks: with hard-priority tasks, the quadratic programming problem for :math:`n` tasks is defined + in a sequential manner, where the first most important task will be first optimized, and then the subsequent tasks + will be optimized one after the other. Thus, the first task to be optimized is given by: -.. math:: x_1^* &= \arg \min_x ||A_1 x - b_1||^2 \\ \text{subj. to} - & G_1 x \leq h_1 \\ - & F_1 x = c_1, + .. math:: -while the second next most important task that would be solved is given by: + x_1^* =& \arg \min_x \; ||A_1 x - b_1||^2 \\ + & \text{subj. to } \; \begin{array}{c} G_1 x \leq h_1 \\ F_1 x = c_1 \end{array} -.. math:: x_2^* &= \arg \min_x ||A_2 x - b_2||^2 \\ \text{subj. to} - & G_2 x \leq h_2 \\ + while the second next most important task that would be solved is given by: + + .. math:: + + x_2^* =& \arg \min_x \; ||A_2 x - b_2||^2 \\ + & \begin{array}{cc} \text{subj. to } & G_2 x \leq h_2 \\ & F_2 x = c_2 \\ & A_1 x = A_1 x_1^* \\ & G_1 x \leq h_1 \\ - & F_1 x = c_1, + & F_1 x = c_1, \end{array} -until the :math:`n` most important task, given by: + until the :math:`n` most important task, given by: -.. math:: x_n^* \arg \min_x ||A_n x - b_n||^2 \\ \text{subj. to} - & A_1 x = A_1 x_1^* \\ + .. math:: + + x_n^* =& \arg \min_x \; ||A_n x - b_n||^2 \\ + & \begin{array}{cc} \text{subj. to } & A_1 x = A_1 x_1^* \\ & ... \\ & A_{n-1} x = A_{n-1} x_{n-1}^* \\ & G_1 x \leq h_1 \\ @@ -78,23 +93,28 @@ until the :math:`n` most important task, given by: & G_n x \leq h_n \\ & F_1 x = c_1 \\ & ... \\ - & F_n x = c_n. + & F_n x = c_n. \end{array} -By setting the previous :math:`A_{i-1} x = A_{i-1} x_{i-1}^*` as equality constraints, the current solution -:math:`x_i^*` won't change the optimality of all higher priority tasks. + By setting the previous :math:`A_{i-1} x = A_{i-1} x_{i-1}^*` as equality constraints, the current solution + :math:`x_i^*` won't change the optimality of all higher priority tasks. +The implementation of this class and the subsequent classes is inspired by [2] (which is licensed under the LGPLv2). + References: - [1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017 - [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 - [3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 + - [1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017 + - [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + - [3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 """ import numpy as np +from pyrobolearn.priorities.models import ModelInterface + + __author__ = "Brian Delhaisse" -__copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["OpenSoT (Enrico Mingo Hoffman and Alessio Rocchi)", "Songyan Xin"] +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Alessio Rocchi (C++)", "Brian Delhaisse (Python + doc)"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" @@ -105,14 +125,33 @@ __status__ = "Development" class Constraint(object): r"""Constraint (abstract) class. - Python implementation of Constraints based on the slides of the OpenSoT framework [1]. + Constraints can be classified into 2 groups: + + 1. inequality constraints + - bounds: math:`lb \leq x \leq ub`. + - bilateral: :math:`b_l \leq A_{ineq} x \leq b_u` + - unilateral: math:`b_l \leq A_{ineq} x` xor :math:`A_{ineq} x \leq b_u` + 2. equality constraints: math:`A_{eq} = b_{eq}`. + + In the robotics case, they can also be divided into 2 groups on another axis: + + 1. kinematics constraints: take only into account kinematic information such as velocities. + 2. dynamic constraints: take into account forces, accelerations, and inertia. + + + When using Quadratic programming, we only consider linear constraints. Non-linear constraints thus have to be + linearized in order to be used. This is for instance the case with a friction cone (which provides a non-linear + constraint) and its linearization; the friction pyramid. + + + Python implementation of Constraints based on the OpenSoT framework [1]. References: - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" ([code](https://opensot.wixsite.com/opensot), [slides](https://docs.google.com/presentation/d/1kwJsAnVi_3ADtqFSTP8wq3JOGLcvDV_ypcEEjPHnCEA), [tutorial video](https://www.youtube.com/watch?v=yFon-ZDdSyg), - [old code](https://github.com/songcheng/OpenSoT)), Rocchi et al., 2015 + [old code](https://github.com/songcheng/OpenSoT), LGPLv2), Rocchi et al., 2015 """ def __init__(self, model): @@ -120,9 +159,9 @@ class Constraint(object): Initialize the Constraint. Args: - model (robot, str): robot model. + model (ModelInterface): model interface. """ - self._model = model + self.model = model ############## # Properties # @@ -130,45 +169,182 @@ class Constraint(object): @property def model(self): - """Return the robot model.""" + """Return the model interface.""" return self._model + @model.setter + def model(self, model): + """Set the model interface.""" + if not isinstance(model, ModelInterface): + raise TypeError("Expecting the given 'model' to be an instance of `ModelInterface`, instead got: " + "{}".format(model)) + self._model = model + @property def lower_bound(self): - """Return the lower bound.""" + r"""Return the lower bound of the optimization variables: :math:`b_l \leq x`.""" return self._lower_bound @property def upper_bound(self): - """Return the upper bound.""" + r"""Return the upper bound of the optimization variables: :math:`x \leq b_u`.""" return self._upper_bound @property def A_eq(self): - """Return the :math:`A_{eq}` matrix.""" + r"""Return the equality constraint matrix :math:`A_{eq}`, such that :math:`A_{eq} x = b_{eq}`.""" return self._A_eq @property def b_eq(self): - """Return the :math:`b_{eq}` vector""" + r"""Return the equality constraint vector :math:`b_{eq}`, such that :math:`A_{eq} x = b_{eq}`.""" return self._b_eq + @property + def A_ineq(self): + r"""Return the inequality constraint matrix :math:`A_{ineq}`, such that :math:`b_l \leq A_{ineq} x \leq b_u`.""" + return self._A_ineq + + @property + def b_lower_bound(self): + r"""Return the lower bound of the inequality constraint: :math:`b_l \leq A_{ineq} x`.""" + return self._b_lower_bound + + # alias + b_ineq_lower = b_lower_bound + + @property + def b_upper_bound(self): + r"""Return the upper bound of the inequality constraint: :math:`A_{ineq} x \leq b_u`.""" + return self._b_upper_bound + + @property + def G(self): + r"""Return the inequality constraint matrix :math:`G` used in inequality constraints :math:`Gx \leq h` in QP.""" + pass + + @property + def h(self): + r"""Return the inequality constraint vector :math:`h` used in inequality constraints :math:`Gx \leq h` in QP.""" + pass + + @property + def F(self): + r"""Return the equality constraint matrix :math:`F` used in equality constraints :math:`Fx = c` in QP.""" + pass + + @property + def c(self): + r"""Return the equality constraint vector :math:`c` used in equality constraints :math:`Fx = c` in QP.""" + + ################## + # Static methods # + ################## + + @staticmethod + def is_equality_constraint(): + r"""Return True if it is an equality constraint: :math:`A_{eq} x = b_{eq}`.""" + return False + + @staticmethod + def is_inequality_constraint(): + r"""Return True if it is an inequality constraint: :math:`b_l \leq A_{ineq} x \leq b_u`.""" + return False + + @staticmethod + def has_bounds(): + r"""Return True if it is a bound constraint: math:`b_l \leq x \leq b_u`.""" + return False + + @staticmethod + def is_unilateral_constraint(): + r"""Return True if it is a unilateral constraint: math:`b_l \leq A_{ineq} x` xor :math:`A_{ineq} x \leq b_u`.""" + return False + + @staticmethod + def is_bilateral_constraint(): + r"""Return True if it is a bilateral constraint: :math:`b_l \leq A_{ineq} x \leq b_u`.""" + return False + ########### # Methods # ########### - def update(self): + def update(self, x): + r""" + Update the various constraint matrices and vectors: :math:`A_{eq}, b_{eq}, A_{ineq}, b_l, b_u, ...`. + + Args: + x (np.array): current optimization variables values. + """ pass ############# # Operators # ############# - def __repr__(self): - return self.__class__.__name__ + # def __repr__(self): + # return self.__class__.__name__ def __str__(self): + """Return a string describing the class.""" return self.__class__.__name__ - def __call__(self): - return self.update() + def __call__(self, x): + """ + Update the constraint (i.e. update the various constraint matrices and vectors. + + Args: + x (np.array): current optimization variable values. + """ + return self.update(x) + + +class EqualityConstraint(Constraint): + r"""Equality constraint""" + pass + + +class InequalityConstraint(Constraint): + r"""Inequality constraint""" + pass + + +class UnilateralConstraint(InequalityConstraint): + r"""Unilateral inequality constraint""" + pass + + +class BilateralConstraint(InequalityConstraint): + r"""Bilateral inequality constraint.""" + pass + + +class BoundConstraint(InequalityConstraint): + r"""Bound inequality constraint.""" + pass + + +class KinematicConstraint(Constraint): + r"""Kinematic constraint.""" + pass + + +class JointVelocityConstraint(KinematicConstraint): + r"""Joint velocity constraint.""" + pass + + +class DynamicConstraint(Constraint): + r"""Dynamic constraint.""" + pass + + +class JointAccelerationConstraint(DynamicConstraint): + r"""Joint acceleration constraint.""" + pass + + +class JointEffortConstraint(DynamicConstraint): + r"""Joint effort constraint.""" + pass diff --git a/pyrobolearn/priorities/constraints/dynamic_constraints.py b/pyrobolearn/priorities/constraints/dynamic_constraints.py index 9aa3b48..e843bae 100644 --- a/pyrobolearn/priorities/constraints/dynamic_constraints.py +++ b/pyrobolearn/priorities/constraints/dynamic_constraints.py @@ -70,6 +70,7 @@ class UnilateralContact(DynamicConstraint): Mechanical constraint which prevents penetration between two bodies. """ + pass class WrenchLimits(DynamicConstraint): diff --git a/pyrobolearn/priorities/constraints/force/__init__.py b/pyrobolearn/priorities/constraints/force/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/priorities/constraints/force/contact.py b/pyrobolearn/priorities/constraints/force/contact.py new file mode 100644 index 0000000..43cf25f --- /dev/null +++ b/pyrobolearn/priorities/constraints/force/contact.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python +r"""Provide the contact (force normal) constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import Constraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Songyan Xin (insight)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class ContactConstraint(Constraint): + r"""Contact force constraint + + The contact force constraint is given by :math:`0 \leq f^i_n` where :math:`f^i_n` is the normal force with respect + to the contact surface applied on the link in contact :math:`i` defined in the local frame. + + References: + - [1] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018 + """ + + def __init__(self, model): + super(ContactConstraint, self).__init__(model) diff --git a/pyrobolearn/priorities/constraints/force/cop.py b/pyrobolearn/priorities/constraints/force/cop.py new file mode 100644 index 0000000..aeeaaea --- /dev/null +++ b/pyrobolearn/priorities/constraints/force/cop.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python +r"""Provide the center of pressure constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import Constraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Arturo Laurenzi (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class CoPConstraint(Constraint): + r"""Center of Pressure (CoP) constraint. + + "The CoP is the point on the ground where the resultant of the ground-reaction force acts". [1] + + This is defined mathematically as: + + .. math:: + + x_{CoP} = \frac{\sum_i x_i f^i_n}{\sum_i f^i_n} + y_{CoP} = \frac{\sum_i y_i f^i_n}{\sum_i f^i_n} + z_{CoP} = \frac{\sum_i z_i f^i_n}{\sum_i f^i_n} + + where :math:`[x_i, y_i, z_i]` are the coordinates of the contact point :math:`i` on which the normal force + :math:`f^i_n` acts. + + Notes: + - the ZMP and CoP are equivalent for horizontal ground surfaces. For irregular ground surfaces they are + distinct. [2] + + References: + - [1] "Postural Stability of Biped Robots and Foot-Rotation Index (FRI) Point", Goswami, 1999 + - [2] "Ground Reference Points in Legged Locomotion: Definitions, Biological Trajectories and Control + Implications", Popovic et al., 2005 + """ + + def __init__(self, model): + super(CoPConstraint, self).__init__(model) diff --git a/pyrobolearn/priorities/constraints/force/friction.py b/pyrobolearn/priorities/constraints/force/friction.py new file mode 100644 index 0000000..bdfa93d --- /dev/null +++ b/pyrobolearn/priorities/constraints/force/friction.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python +r"""Provide the friction cone (nonlinear) and pyramid (linear) constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + - [2] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import Constraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Alessio Rocchi (C++)", "Songyan Xin (insight)", + "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class FrictionConeConstraint(Constraint): + r"""Friction Cone constraint + + The friction cone is defined as: + + .. math:: C^i_s = {(f^i_x, f^i_y, f^i_z) \in \mathbb{R}^3 | \sqrt{(f^i_x)^2 + (f^i_y)^2} \leq \mu_i f^i_z } + + where :math:`i` denotes the ith support/contact, :math:`f^i_s` is the contact spatial force exerted at + the contact point :math:`C_i`, and :math:`\mu_i` is the static friction coefficient at that contact point. + + "A point contact remains in the fixed contact mode while its contact force f^i lies inside the friction cone" + [1]. Often, the friction pyramid which is the linear approximation of the friction cone is considered as it + is easier to manipulate it; e.g. present it as a linear constraint in a quadratic optimization problem. + + References: + - [1] https://scaron.info/teaching/friction-cones.html + - [2] "Stability of Surface Contacts for Humanoid Robots: Closed-Form Formulae of the Contact Wrench Cone + for Rectangular Support Areas", Caron et al., 2015 + - [3] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + - [4] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018 + """ + + def __init__(self, model): + super(FrictionConeConstraint, self).__init__(model) + + +class FrictionPyramidConstraint(Constraint): + r"""Friction Pyramid constraint + + The friction pyramid constraint is a linear approximation of the friction cone. + + The friction pyramid is defined as: + + .. math:: P^i_s = {(f^i_x, f^i_y, f^i_z) \in \mathbb{R}^3 | f^i_x \leq \mu_i f^i_z, f^i_y \leq \mu_i f^i_z} + + where where :math:`i` denotes the ith support/contact, :math:`f^i_s` is the contact spatial force exerted at + the contact point :math:`C_i`, and :math:`\mu_i` is the static friction coefficient at that contact point. + If the static friction coefficient is given by :math:`\frac{\mu_i}{\sqrt{2}}`, then we are making an inner + approximation (i.e. the pyramid is inside the cone) instead of an outer approximation (i.e. the cone is inside + the pyramid). [1] + + This linear approximation is often used as a linear constraint in a quadratic optimization problem along with + the unilateral constraint :math:`f^i_z \geq 0`. + + References: + - [1] https://scaron.info/teaching/friction-cones.html + - [2] "Stability of Surface Contacts for Humanoid Robots: Closed-Form Formulae of the Contact Wrench Cone + for Rectangular Support Areas", Caron et al., 2015 + """ + + def __init__(self, model): + super(FrictionPyramidConstraint, self).__init__(model) \ No newline at end of file diff --git a/pyrobolearn/priorities/constraints/force/wrench_limits.py b/pyrobolearn/priorities/constraints/force/wrench_limits.py new file mode 100644 index 0000000..438393e --- /dev/null +++ b/pyrobolearn/priorities/constraints/force/wrench_limits.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python +r"""Provide the wrench limits constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import Constraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class WrenchLimitsConstraint(Constraint): + r"""Wrench Limits constraint. + + """ + + def __init__(self, model): + super(WrenchLimitsConstraint, self).__init__(model) diff --git a/pyrobolearn/priorities/constraints/force/zmp.py b/pyrobolearn/priorities/constraints/force/zmp.py new file mode 100644 index 0000000..3323a65 --- /dev/null +++ b/pyrobolearn/priorities/constraints/force/zmp.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python +r"""Provide the Zero-Moment Point constraint. + + +References: + - [1] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import Constraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Songyan Xin"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class ZMPConstraint(Constraint): + r"""Zero-Moment Point constraint. + + "The ZMP is the point on the ground surface about which the horizontal component of the moment of ground + reaction force is zero. It resolves the ground reaction force distribution to a single point." [1] + + Assumptions: the contact area is planar and has sufficiently high friction to keep the feet from sliding. + + .. math:: + + x_{ZMP} &= x_{CoM} - \frac{F_x}{F_z + Mg} z_{CoM} - \frac{\tau_{y}(\vec{r}_{CoM})}{F_z + Mg} \\ + y_{ZMP} &= y_{CoM} - \frac{F_y}{F_z + Mg} z_{CoM} + \frac{\tau_{x}(\vec{r}_{CoM})}{F_z + Mg} + + where :math:`[x_{CoM}, y_{CoM}, z_{CoM}]` is the center of mass position, :math:`M` is the body mass, + :math:`g` is the gravity value, :math:`F = Ma_{CoM}` is the net force acting on the whole body (including the + gravity force :math:`-Mg`), :math:`\vec{r}_{CoM}` is the body center of mass, and :math:`\tau(\vec{r}_{CoM})` + is the net whole-body moment about the center of mass. + + In the case where there are only ground reaction forces (+ the gravity force) acting on the robot, then the + ZMP point is given by [3]: + + .. math:: + + x_{ZMP} &= x_{CoM} - \frac{F_{G.R.X}}{F_{G.R.Z}} z_{CoM} - \frac{\tau_{y}(\vec{r}_{CoM})}{F_{G.R.Z}} \\ + y_{ZMP} &= y_{CoM} - \frac{F_{G.R.Y}}{F_{G.R.Z}} z_{CoM} + \frac{\tau_{x}(\vec{r}_{CoM})}{F_{G.R.Z}} + + where :math:`F_{G.R}` are the ground reaction forces, and the net moment about the CoM + :math:`\tau(\vec{r}_{CoM})` is computed using the ground reaction forces. + + The ZMP constraints can be expressed as: + + .. math:: + + d_x^{-} \leq \frac{n^i_y}{f^i_z} \leq d_x^{+} \\ + d_y^{-} \leq -\frac{n^i_x}{f^i_z} \leq d_y^{+} + + which ensures the stability of the foot/ground contact. The :math:`(d_x^{-}, d_x^{+})` and + :math:`(d_y^{-}, d_y^{+})` defines the size of the sole in the x and y directions respectively. Basically, + this means that the ZMP point must be inside the convex hull in order to have a static stability. + The :math:`n^i` are the contact spatial torques around the contact point :math:`i`, and :math:`f` is the + contact spatial force at the contact point :math:`i`. + + Notes: + - the ZMP and CoP are equivalent for horizontal ground surfaces. For irregular ground surfaces they are + distinct. [1] + - the FRI coincides with the ZMP when the foot is stationary. [1] + - the CMP coincides with the ZMP, when the moment about the CoM is zero. [1] + + References: + - [1] "Ground Reference Points in Legged Locomotion: Definitions, Biological Trajectories and Control + Implications", Popovic et al., 2005 + - [2] "Biped Walking Pattern Generation by using Preview Control of ZMP", Kajita et al., 2003 + - [3] "Exploiting Angular Momentum to Enhance Bipedal Center-of-Mass Control", Hofmann et al., 2009 + """ + + def __init__(self, model): + super(ZMPConstraint, self).__init__(model) diff --git a/pyrobolearn/priorities/constraints/torque/__init__.py b/pyrobolearn/priorities/constraints/torque/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/priorities/constraints/torque/joint_limits.py b/pyrobolearn/priorities/constraints/torque/joint_limits.py new file mode 100644 index 0000000..0fbb563 --- /dev/null +++ b/pyrobolearn/priorities/constraints/torque/joint_limits.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python +r"""Provide the joint limits constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import Constraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Alessio Rocchi (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class JointLimitsConstraint(Constraint): + r"""Joint Limits constraint. + + """ + + def __init__(self, model): + super(JointLimitsConstraint, self).__init__(model) diff --git a/pyrobolearn/priorities/constraints/torque/torque_limits.py b/pyrobolearn/priorities/constraints/torque/torque_limits.py new file mode 100644 index 0000000..2b6ac5e --- /dev/null +++ b/pyrobolearn/priorities/constraints/torque/torque_limits.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python +r"""Provide the torque limits constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import Constraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class TorqueLimitsConstraint(Constraint): + r"""Torque Limits constraint. + + """ + + def __init__(self, model): + super(TorqueLimitsConstraint, self).__init__(model) diff --git a/pyrobolearn/priorities/constraints/velocity/__init__.py b/pyrobolearn/priorities/constraints/velocity/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/priorities/constraints/velocity/capture_point.py b/pyrobolearn/priorities/constraints/velocity/capture_point.py new file mode 100644 index 0000000..47b2973 --- /dev/null +++ b/pyrobolearn/priorities/constraints/velocity/capture_point.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python +r"""Provide the capture point constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import Constraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class CapturePointConstraint(Constraint): + r"""Capture Point constraint. + + """ + + def __init__(self, model): + super(CapturePointConstraint, self).__init__(model) diff --git a/pyrobolearn/priorities/constraints/velocity/cartesian_position.py b/pyrobolearn/priorities/constraints/velocity/cartesian_position.py new file mode 100644 index 0000000..83a9eed --- /dev/null +++ b/pyrobolearn/priorities/constraints/velocity/cartesian_position.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python +r"""Provide the Cartesian Position constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import Constraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Alessio Rocchi (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class CartesianPositionConstraint(Constraint): + r"""Cartesian Position constraint. + + """ + + def __init__(self, model): + super(CartesianPositionConstraint, self).__init__(model) diff --git a/pyrobolearn/priorities/constraints/velocity/cartesian_velocity.py b/pyrobolearn/priorities/constraints/velocity/cartesian_velocity.py new file mode 100644 index 0000000..4d2c1b9 --- /dev/null +++ b/pyrobolearn/priorities/constraints/velocity/cartesian_velocity.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python +r"""Provide the cartesian velocity constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import Constraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Alessio Rocchi (C++)", "Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class CartesianVelocityConstraint(Constraint): + r"""Cartesian Velocity constraint. + + """ + + def __init__(self, model): + super(CartesianVelocityConstraint, self).__init__(model) diff --git a/pyrobolearn/priorities/constraints/velocity/com_velocity.py b/pyrobolearn/priorities/constraints/velocity/com_velocity.py new file mode 100644 index 0000000..9f6ec51 --- /dev/null +++ b/pyrobolearn/priorities/constraints/velocity/com_velocity.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python +r"""Provide the Center of Mass velocity constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import Constraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Alessio Rocchi (C++)", "Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class CoMVelocityConstraint(Constraint): + r"""Center of Mass Velocity constraint. + + """ + + def __init__(self, model): + super(CoMVelocityConstraint, self).__init__(model) diff --git a/pyrobolearn/priorities/constraints/velocity/convex_hull.py b/pyrobolearn/priorities/constraints/velocity/convex_hull.py new file mode 100644 index 0000000..faab44a --- /dev/null +++ b/pyrobolearn/priorities/constraints/velocity/convex_hull.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python +r"""Provide the Convex Hull constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import Constraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Alessio Rocchi (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class ConvexHullConstraint(Constraint): + r"""Convex Hull constraint. + + """ + + def __init__(self, model): + super(ConvexHullConstraint, self).__init__(model) diff --git a/pyrobolearn/priorities/constraints/velocity/dynamics.py b/pyrobolearn/priorities/constraints/velocity/dynamics.py new file mode 100644 index 0000000..772d91c --- /dev/null +++ b/pyrobolearn/priorities/constraints/velocity/dynamics.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python +r"""Provide the dynamics constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import Constraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["OpenSoT (Alessio Rocchi and Enrico Mingo Hoffman, C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class DynamicsConstraint(Constraint): + r"""Dynamics constraint. + + """ + + def __init__(self, model): + super(DynamicsConstraint, self).__init__(model) diff --git a/pyrobolearn/priorities/constraints/velocity/joint_limits.py b/pyrobolearn/priorities/constraints/velocity/joint_limits.py new file mode 100644 index 0000000..8deb87b --- /dev/null +++ b/pyrobolearn/priorities/constraints/velocity/joint_limits.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python +r"""Provide the joint position limits constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import BoundConstraint, JointVelocityConstraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Alessio Rocchi (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class JointPositionLimitsConstraint(BoundConstraint, JointVelocityConstraint): + r"""Joint Position Limits constraint. + + This provides bounds/limits on the joint positions, which are given by: + + .. math:: q_{lb} \leq q + \dot{q} dt \leq q_{ub} + + where :math:`(q_{lb}, q_{ub})` are the lower and upper bound of the joint positions respectively, :math:`q` are + the current joint positions, :math:`\dot{q}` are the joint velocities being optimized, and :math:`dt` is the + integration time step. + + This formulation can be rewritten as the inequality constraint :math:`Gx \leq h` used in QP, with + :math:`G = [-dt*I, dt*I]^\top` and :math:`h = [(q - q_{lb})^\top, (q_{ub} - q)^\top]^\top` where :math:`I` is the + square identity matrix. + """ + + def __init__(self, model, dt): + r""" + Initialize the constraint. + + Args: + model (ModelInterface): model interface. + dt (float): integration time step: use to compute :math:`q = q + \dot{q} dt`. + """ + super(JointPositionLimitsConstraint, self).__init__(model) + + self.dt = dt + + bounds = self.model.get_joint_bounds() + self.lower_bound = bounds[0] + self.upper_bound = bounds[1] + + def update(self): + r""" + Update the bounds. + """ + q = self.model.get_joint_positions() + self.lower_bound = 0 + self.upper_bound = 0 diff --git a/pyrobolearn/priorities/constraints/velocity/joint_velocity.py b/pyrobolearn/priorities/constraints/velocity/joint_velocity.py new file mode 100644 index 0000000..1b7ee87 --- /dev/null +++ b/pyrobolearn/priorities/constraints/velocity/joint_velocity.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python +r"""Provide the differential kinematics constraint. + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import EqualityConstraint, JointVelocityConstraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class DifferentialKinematicsConstraint(EqualityConstraint, JointVelocityConstraint): + r"""Differential kinematics constraint. + + This provides the joint velocity constraints which is given by: + + .. math:: J(q) \dot{q} = v + + where :math:`J(q)` is the jacobian from a base link to a distal link, :math:`\dot{q}` are the joint velocities + being optimized, and :math:`v` is the imposed cartesian velocity imposed on the distal link. + + This formulation can be rewritten as the inequality constraint :math:`A_{eq} x = b_{eq}` used in QP, with + :math:`A_{eq} = J(q)`, :math:`x = \dot{q}`, and :math:`b_{eq} = v`. + """ + + def __init__(self, model, distal_link, base_link=None, velocity=None): + r""" + Initialize the constraint. + + Args: + model (ModelInterface): model interface. + distal_link (int, str): distal link id or name. + base_link (int, str, None): base link id or name. If None, it will be the world. + velocity (np.array[6], None): imposed velocity. If None, it will be set to 0. + """ + super(DifferentialKinematicsConstraint, self).__init__(model) + raise NotImplementedError + + def update(self): + r""" + Update the bounds. + """ + raise NotImplementedError diff --git a/pyrobolearn/priorities/constraints/velocity/self_collision_avoidance.py b/pyrobolearn/priorities/constraints/velocity/self_collision_avoidance.py new file mode 100644 index 0000000..604b996 --- /dev/null +++ b/pyrobolearn/priorities/constraints/velocity/self_collision_avoidance.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python +r"""Provide the self collision avoidance constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import Constraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Cheng Fang (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class SelfCollisionAvoidanceConstraint(Constraint): + r"""Self Collision Avoidance constraint. + + """ + + def __init__(self, model): + super(SelfCollisionAvoidanceConstraint, self).__init__(model) diff --git a/pyrobolearn/priorities/constraints/velocity/velocity_limits.py b/pyrobolearn/priorities/constraints/velocity/velocity_limits.py new file mode 100644 index 0000000..ea1d890 --- /dev/null +++ b/pyrobolearn/priorities/constraints/velocity/velocity_limits.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python +r"""Provide the velocity limits constraint. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.constraints.constraint import BoundConstraint, JointVelocityConstraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Alessio Rocchi (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class JointVelocityLimitsConstraint(BoundConstraint, JointVelocityConstraint): + r"""Joint velocity limits constraint. + + This provides bounds/limits on the joint velocities + + .. math:: \dot{q}_{lb} \leq \dot{q} \leq \dot{q}_{ub} + + where :math:`(\dot{q}_{lb}, \dot{q}_{ub})` are the lower and upper bound on the joint velocities, and + :math:`\dot{q}` are the joint velocities being optimized. + + This formulation can be rewritten as the inequality constraint :math:`Gx \leq h` used in QP, with + :math:`G = [-I, I]^\top` and :math:`h = [-q_{lb}^\top, q_{ub}^\top]^\top` where :math:`I` is the square identity + matrix. + """ + + def __init__(self, model): + """ + Initialize the constraint. + + Args: + model (ModelInterface): model interface. + """ + super(JointVelocityLimitsConstraint, self).__init__(model) + + bounds = self.model.get_joint_velocity_bounds() + self.lower_bound = bounds[0] + self.upper_bound = bounds[1] + + def update(self): + pass diff --git a/pyrobolearn/priorities/model.py b/pyrobolearn/priorities/model.py deleted file mode 100644 index 60e6bc8..0000000 --- a/pyrobolearn/priorities/model.py +++ /dev/null @@ -1,111 +0,0 @@ -#!/usr/bin/env python -r"""Model interface used in priority tasks. - -This is based on the implementation in `https://github.com/ADVRHumanoids/ModelInterfaceRBDL`. - -References: - [1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017 - [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 - [3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 -""" - -import numpy as np -import rbdl - - -__author__ = "Brian Delhaisse" -__copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["OpenSoT (Enrico Mingo Hoffman and Alessio Rocchi)", "Songyan Xin"] -__license__ = "GNU GPLv3" -__version__ = "1.0.0" -__maintainer__ = "Brian Delhaisse" -__email__ = "briandelhaisse@gmail.com" -__status__ = "Development" - - -class ModelInterface(object): - r"""Model interface. - - """ - - def __init__(self, urdf): - self.model = rbdl.loadModel(filename=urdf) - self.model = rbdl.Model() - self.q = np.zeros(self.model.q_size) - self.dq = np.zeros(self.model.qdot_size) - self.ddq = np.zeros(self.model.qdot_size) - self.mass = 0 - self.com = np.zeros(3) - self.com_vel = np.zeros(3) - self.com_acc = np.zeros(3) - self.angular_momentum_com = np.zeros(3) - self.change_angular_momentum_com = np.zeros(3) - - @property - def num_dof(self): - return self.model.dof_count - - def get_com(self): - return rbdl.CalcCenterOfMass(self.model, self.q, self.dq, self.ddq, self.com, self.com_vel, self.com_acc, - self.angular_momentum_com, self.change_angular_momentum_com, - update_kinematics=True) - - def get_com_jacobian(self): - pass - - def get_com_velocity(self): - pass - - def get_com_acceleration(self): - pass - - def get_gravity(self): - pass - - def get_jacobian(self): - pass - - def get_pose(self): - pass - - def get_acceleration_twist(self): - pass - - def get_velocity_twist(self): - pass - - def set_floating_base_pose(self): - pass - - def set_floating_base_twist(self): - pass - - def set_gravity(self): - pass - - def compute_gravity_compensation(self): - pass - - def get_centroidal_momentum(self): - pass - - def compute_inverse_dynamics(self): - pass - - def compute_non_linear_term(self): - pass - - def get_inertia_matrix(self): - pass - - def get_link_id(self, link_name): - pass - - def update(self, q=None, dq=None, ddq=None): - if q is None: - q = self.q - if dq is None: - dq = self.dq - if ddq is None: - ddq = self.ddq - rbdl.UpdateKinematics(self.model, q, dq, ddq) diff --git a/pyrobolearn/priorities/models/README.rst b/pyrobolearn/priorities/models/README.rst new file mode 100644 index 0000000..f5dfc5a --- /dev/null +++ b/pyrobolearn/priorities/models/README.rst @@ -0,0 +1,16 @@ +Model Interfaces +================ + +In this folder, we provide the model interfaces that are used and shared by the various tasks and constraints. + +A model interface is an abstraction layer that provides a common interface and remove the direct coupling between the +``Robot`` class and the classes (tasks, constraints, solvers) defined in ``pyrobolearn/priorities``. Additionally, it +also serves as a container to different quantities (joint positions, velocities, torques, etc), which avoids the need +to recompute them for each task / constraint that used them. + +The model interface API has been heavily inspired (mostly translated from C++ to Python) by the interface provided in +[1]_ and has been improved. + +References: + +.. [1] https://github.com/ADVRHumanoids/ModelInterfaceRBDL diff --git a/pyrobolearn/priorities/models/__init__.py b/pyrobolearn/priorities/models/__init__.py new file mode 100644 index 0000000..5dbd058 --- /dev/null +++ b/pyrobolearn/priorities/models/__init__.py @@ -0,0 +1,9 @@ + +# import the abstract model interface +from .model import ModelInterface + +# import the rbdl model interface +from .rbdl_model import RBDLModelInterface + +# import the robot model interface +from .robot_model import RobotModelInterface diff --git a/pyrobolearn/priorities/models/model.py b/pyrobolearn/priorities/models/model.py new file mode 100644 index 0000000..6a81989 --- /dev/null +++ b/pyrobolearn/priorities/models/model.py @@ -0,0 +1,378 @@ +#!/usr/bin/env python +r"""Model interface used in priority tasks. + +A model interface is an abstraction layer that provides a common interface and remove the direct coupling between the +``Robot`` and the tasks, constraints, and solvers. It also serves as a container to different quantities (joint +positions, velocities, torques, etc), which avoids the need to recompute them for each task / constraint that used +them. + +This is based on the implementation in `https://github.com/ADVRHumanoids/ModelInterfaceRBDL` (distributed under the +LGPLv3). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + - [2] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 + - [3] "Rigid Body Dynamics Algorithms", Featherstone, 2008 +""" + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Arturo Laurenzi (C++)", "Luca Muratore (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class ModelInterface(object): + r"""Model interface. + + A model interface is an abstraction layer that provides a common interface and remove the direct coupling between + the ``Robot`` and the tasks, constraints, and solvers. It also serves as a container to different quantities (joint + positions, velocities, torques, etc), which avoids the need to recompute them for each task / constraint that used + them. + + This is based on the implementation in `https://github.com/ADVRHumanoids/ModelInterfaceRBDL` (distributed under + the LGPLv3). + """ + + def __init__(self, model): + # set the model + self.model = model + + # save the states: every time a method (which returned the result) is called, the result is cached here + # avoiding to recompute it + self.states = dict() + + ############## + # Properties # + ############## + + @property + def gravity(self): + """Get the gravity vector.""" + return self.get_gravity() + + # @gravity.setter + # def gravity(self, gravity): + # """Set the gravity vector.""" + # self.set_gravity(gravity) + + @property + def num_dofs(self): + """Return the number of degrees of freedom.""" + raise NotImplementedError + + ########### + # Methods # + ########### + + def get_joint_positions(self): + """ + Get the joint positions. + + Returns: + np.array[N]: the joint positions. + """ + pass + + def get_joint_velocities(self): + """ + Get the joint velocities. + + Returns: + np.array[N]: the joint positions. + """ + pass + + def get_joint_accelerations(self): + """ + Get the joint accelerations. + + Returns: + np.array[N]: the joint positions. + """ + pass + + def get_com_position(self): + """ + Get the position of the center of mass (CoM). + + Returns: + np.array[3]: position of the center of mass + """ + pass + + def get_com_jacobian(self): + """ + Get the CoM Jacobian. + + Returns: + np.array[N,N]: CoM Jacobian (where N is the number of DoFs) + """ + pass + + def get_com_velocity(self): + """ + Get the linear CoM velocity. + + Returns: + np.array[3]: CoM velocity. + """ + pass + + def get_com_acceleration(self): + """ + Get the linear CoM acceleration. + + Returns: + np.array[3]: CoM acceleration. + """ + pass + + def get_gravity(self): + """ + Get the gravity vector applied on the model. + + Returns: + np.array[3]: gravity vector expressed in the world frame. + """ + pass + + def _set_gravity(self, gravity): + """ + Set the gravity vector applied on the model. + + Args: + gravity (np.array[3]): gravity vector expressed in the world frame. + """ + pass + + def get_model_ordered_joint_names(self): + """ + Get the model ordered joint names. + + Returns: + list of str: list of joint names. + """ + pass + + def get_jacobian(self, link_id, point): + r""" + Get the 6D Jacobian for a point on a link, that when multiplied with :math:`\dot{q}` gives a 6D vector that + has the angular velocity as the first three entries and the linear velocity as the last three entries. + + .. math:: v = [\omega, \dot{p}] = J(q) \dot{q} + + where :math:`J(q)` is the concatenation of the angular and linear Jacobian. + + Args: + link_id (int): unique link id. + point (np.array[3]): position of the point in link's local frame + + Returns: + np.array[6, N]: 6D Jacobian (=concatenation of the angular and linear Jacobian). + """ + pass + + def get_pose(self, link_id): + """ + Return the pose of the specified link. + + Args: + link_id (int): link id + + Returns: + np.array[7]: pose (position and quaternion expressed as [x,y,z,w]) + """ + pass + + def get_velocity_twist(self, link_id): + r""" + Compute the angular and linear velocity of a link, given by :math:`v = [\omega, \dot{p}]`. + + Args: + link_id (int): link id. + + Returns: + np.array[6]: The resulting 6D spatial velocity vector where the first three elements are the angular + velocity and the last three are the linear velocity expressed in the global world reference frame. + """ + pass + + def get_acceleration_twist(self, link_id): + r""" + Compute the angular and linear acceleration of a link, given by :math:`\dot{v} = [dot{\omega}, \ddot{p}]`. + + Args: + link_id (int): link id. + + Returns: + np.array[6]: The resulting 6D spatial acceleration vector where the first three elements are the + angular acceleration and the last three are the linear acceleration expressed in the global world + reference frame. + """ + pass + + def get_relative_acceleration_twist(self, target_link_id, base_link_id): + r""" + Compute the relative angular and linear acceleration of a target link with respect to a base link. The + acceleration is given by :math:`\dot{v} = [dot{\omega}, \ddot{p}]`. + + Returns: + np.array[6]: The resulting 6D spatial acceleration vector where the first three elements are the + angular acceleration and the last three are the linear acceleration expressed in the local frame of + the base link. + """ + pass + + def set_floating_base_pose(self, pose): + """ + Set the floating base pose. Given the desired pose (=position + orientation), the corresponding joint position + values for the 6 virtual joints (attached to the floating base) are computed. + + Args: + pose (np.array[7]): the desired pose (position and orientation given as a quaternion [x,y,z,w]) of the + floating base. + """ + pass + + def set_floating_base_velocity(self, velocity): + """ + Set the floating base velocity. This computes the corresponding joint velocity values for the 6 virtual joints + (that are attached to the floating base). + + Args: + np.array[3], np.array[6]: desired linear (and angular) velocity of the floating base. + """ + pass + + def compute_gravity_compensation(self): + """ + Return the torques to perform gravity compensation. + + Returns: + np.array[N]: torques to perform gravity compensation. + """ + pass + + def compute_nonlinear_term(self): + r""" + Computes the non-linear terms :math:`C(q, \dot{q})` in the dynamic equation of motion for a rigid-body system, + given by: + + .. math:: \tau = H(q) \ddot{q} + C(q, \dot{q}) + + where ":math:`\tau` is the vector of applied forces, :math:`H` is the joint space inertia matrix, + :math:`C(q, \dot{q})` is the vector of force terms that account for the Coriolis and centrifugal forces, + gravity, and any other forces acting on the system other than those in :math:`\tau`." [1] + + Returns: + np.array[N]: non-linear force terms. + + References: + - [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008 + """ + pass + + def compute_JdotQdot(self): + r""" + Compute :math:`\dot{J}(q) \dot{q}`, which appears in :math:`\dot{v} = J(q) \ddot{q} + \dot{J}(q) \dot{q}`, + which is the first time derivative of :math:`v = J(q) \dot{q}`. + + Returns: + np.array[6]: the matrix multiplication of the first derivative of the Jacobian with the joint velocities. + """ + pass + + def compute_relative_JdotQdot(self, target_link_id, base_link_id): + r""" + Compute the relative :math:`\dot{J}(q) \dot{q}`, which appears in + :math:`\dot{v} = J(q) \ddot{q} + \dot{J}(q) \dot{q}`, which is the first time derivative of + :math:`v = J(q) \dot{q}`. The Jacobian is taken from the specified base link to the target link. + + Args: + target_link_id (int): target link id + base_link_id (int): base link id + + Returns: + np.array[6]: relative :math:`\dot{J}(q) \dot{q}` + """ + pass + + def get_inertia_matrix(self): + """ + Computes the joint space inertia matrix. + + Returns: + np.array[N, N]: joint space inertia matrix (where `N` is the number of DoFs). + """ + pass + + def get_inertia_inverse_times_vector(self, vector): + r""" + Computes the effect of multiplying the inverse of the joint space inertia matrix :math:`H(q)` with a vector + in linear time. + + Args: + np.array[N]: vector to be multiplied with the inverse joint space inertia matrix. + + Returns: + np.array[N]: resulting vector + """ + pass + + def get_point_acceleration(self, link_id, point): + """ + Computes the linear acceleration of a point on a link. + + Args: + link_id (int): unique link id. + point (np.array[3]): position of the point in link's local frame + + Returns: + np.array[3]: The cartesian acceleration of the point in global frame + """ + pass + + def update(self): + """ + This is to notify the model interface that we moved to the next time step :math:`t \rightarrow t+1`. + Practically, it frees every variables that has been cached in this instance (in self.states). + """ + self.states = dict() + self.get_joint_positions() + self.get_joint_velocities() + self.get_joint_accelerations() + + def get_link_id(self, name): + """ + Return the link id associated with the given name. + + Args: + name (str): name of the link + + Returns: + int: unique link id + """ + pass + + def get_mass(self): + """ + Return the total mass of the model. + + Returns: + float: total mass + """ + pass + + def get_floating_base_link(self): + """ + Return the floating base link. + + Returns: + int: floating base link + """ + pass diff --git a/pyrobolearn/priorities/models/rbdl_model.py b/pyrobolearn/priorities/models/rbdl_model.py new file mode 100644 index 0000000..6a00952 --- /dev/null +++ b/pyrobolearn/priorities/models/rbdl_model.py @@ -0,0 +1,367 @@ +#!/usr/bin/env python +r"""RBDL model interface used in priority tasks. + +This is based on the implementation in `https://github.com/ADVRHumanoids/ModelInterfaceRBDL`, which is licensed under +the LPGLv3. + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + - [2] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 + - [3] "Rigid Body Dynamics Algorithms", Featherstone, 2008 +""" + +import numpy as np +import rbdl + +from pyrobolearn.priorities.models import ModelInterface + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Arturo Laurenzi (C++)", "Luca Muratore (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class RBDLModelInterface(ModelInterface): + r"""RBDL Model interface. + + """ + + def __init__(self, urdf, floating_base=False, verbose=False): + """ + Initialize the RBDL model interface. + + Args: + urdf (str): path to the URDF file. + floating_base (bool): set this variable to True, if we have a floating-based robot. + verbose (bool): if True, it will print information when loading the URDF. + """ + # load the RBDL model + model = rbdl.loadModel(filename=urdf, floating_base=floating_base, verbose=verbose) + + # call parent constructor + super(RBDLModelInterface, self).__init__(model) + + # define joint attributes + self.q = np.zeros(self.model.q_size) + self.dq = np.zeros(self.model.qdot_size) + self.ddq = np.zeros(self.model.qdot_size) + + self.mass = 0 + self.com = np.zeros(3) + self.com_vel = np.zeros(3) + self.com_acc = np.zeros(3) + self.angular_momentum_com = np.zeros(3) + self.change_angular_momentum_com = np.zeros(3) + + ############## + # Properties # + ############## + + @property + def model(self): + """Return the model instance.""" + return self._model + + @model.setter + def model(self, model): + """Set the model instance.""" + if not isinstance(model, rbdl.Model): + raise TypeError("Expectig the given 'model' to be an instance of `rbdl.Model`, instead got: " + "{}".format(type(model))) + self._model = model + + @property + def num_dof(self): + """Return the number of degrees of freedom.""" + return self.model.dof_count + + ########### + # Methods # + ########### + + def get_com_position(self): + """ + Get the position of the center of mass (CoM). + + Returns: + np.array[3]: position of the center of mass + """ + return rbdl.CalcCenterOfMass(self.model, self.q, self.dq, self.ddq, self.com, self.com_vel, self.com_acc, + self.angular_momentum_com, self.change_angular_momentum_com, + update_kinematics=True) + + def get_com_jacobian(self): + """ + Get the CoM Jacobian. + + Returns: + np.array[N,N]: CoM Jacobian (where N is the number of DoFs) + """ + pass + + def get_com_velocity(self): + """ + Get the linear CoM velocity. + + Returns: + np.array[3]: CoM velocity. + """ + pass + + def get_com_acceleration(self): + """ + Get the linear CoM acceleration. + + Returns: + np.array[3]: CoM acceleration. + """ + pass + + def get_gravity(self): + """ + Get the gravity vector applied on the model. + + Returns: + np.array[3]: gravity vector expressed in the world frame. + """ + pass + + def set_gravity(self, gravity): + """ + Set the gravity vector applied on the model. + + Args: + gravity (np.array[3]): gravity vector expressed in the world frame. + """ + pass + + def get_model_ordered_joint_names(self): + """ + Get the model ordered joint names. + + Returns: + list of str: list of joint names. + """ + pass + + def get_jacobian(self, link_id, point): + r""" + Get the 6D Jacobian for a point on a link, that when multiplied with :math:`\dot{q}` gives a 6D vector that + has the angular velocity as the first three entries and the linear velocity as the last three entries. + + .. math:: v = [\omega, \dot{p}] = J(q) \dot{q} + + where :math:`J(q)` is the concatenation of the angular and linear Jacobian. + + Args: + link_id (int): unique link id. + point (np.array[3]): position of the point in link's local frame + + Returns: + np.array[6, N]: 6D Jacobian (=concatenation of the angular and linear Jacobian). + """ + pass + + def get_pose(self, link_id): + """ + Return the pose of the specified link. + + Args: + link_id (int): link id + + Returns: + np.array[7]: pose (position and quaternion expressed as [x,y,z,w]) + """ + pass + + def get_velocity_twist(self, link_id): + r""" + Compute the angular and linear velocity of a link, given by :math:`v = [\omega, \dot{p}]`. + + Args: + link_id (int): link id. + + Returns: + np.array[6]: The resulting 6D spatial velocity vector where the first three elements are the angular + velocity and the last three are the linear velocity expressed in the global world reference frame. + """ + pass + + def get_acceleration_twist(self, link_id): + r""" + Compute the angular and linear acceleration of a link, given by :math:`\dot{v} = [dot{\omega}, \ddot{p}]`. + + Args: + link_id (int): link id. + + Returns: + np.array[6]: The resulting 6D spatial acceleration vector where the first three elements are the + angular acceleration and the last three are the linear acceleration expressed in the global world + reference frame. + """ + pass + + def get_relative_acceleration_twist(self, target_link_id, base_link_id): + r""" + Compute the relative angular and linear acceleration of a target link with respect to a base link. The + acceleration is given by :math:`\dot{v} = [dot{\omega}, \ddot{p}]`. + + Returns: + np.array[6]: The resulting 6D spatial acceleration vector where the first three elements are the + angular acceleration and the last three are the linear acceleration expressed in the local frame of + the base link. + """ + pass + + def set_floating_base_pose(self, pose): + """ + Set the floating base pose. Given the desired pose (=position + orientation), the corresponding joint position + values for the 6 virtual joints (attached to the floating base) are computed. + + Args: + pose (np.array[7]): the desired pose (position and orientation given as a quaternion [x,y,z,w]) of the + floating base. + """ + pass + + def set_floating_base_velocity(self, velocity): + """ + Set the floating base velocity. This computes the corresponding joint velocity values for the 6 virtual joints + (that are attached to the floating base). + + Args: + np.array[3], np.array[6]: desired linear (and angular) velocity of the floating base. + """ + pass + + def compute_gravity_compensation(self): + """ + Return the torques to perform gravity compensation. + + Returns: + np.array[N]: torques to perform gravity compensation. + """ + pass + + def compute_nonlinear_term(self): + r""" + Computes the non-linear terms :math:`C(q, \dot{q})` in the dynamic equation of motion for a rigid-body system, + given by: + + .. math:: \tau = H(q) \ddot{q} + C(q, \dot{q}) + + where ":math:`\tau` is the vector of applied forces, :math:`H` is the joint space inertia matrix, + :math:`C(q, \dot{q})` is the vector of force terms that account for the Coriolis and centrifugal forces, + gravity, and any other forces acting on the system other than those in :math:`\tau`." [1] + + Returns: + np.array[N]: non-linear force terms. + + References: + - [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008 + """ + pass + + def compute_JdotQdot(self): + r""" + Compute :math:`\dot{J}(q) \dot{q}`, which appears in :math:`\dot{v} = J(q) \ddot{q} + \dot{J}(q) \dot{q}`, + which is the first time derivative of :math:`v = J(q) \dot{q}`. + + Returns: + np.array[6]: the matrix multiplication of the first derivative of the Jacobian with the joint velocities. + """ + pass + + def compute_relative_JdotQdot(self, target_link_id, base_link_id): + r""" + Compute the relative :math:`\dot{J}(q) \dot{q}`, which appears in + :math:`\dot{v} = J(q) \ddot{q} + \dot{J}(q) \dot{q}`, which is the first time derivative of + :math:`v = J(q) \dot{q}`. The Jacobian is taken from the specified base link to the target link. + + Args: + target_link_id (int): target link id + base_link_id (int): base link id + + Returns: + np.array[6]: relative :math:`\dot{J}(q) \dot{q}` + """ + pass + + def get_inertia_matrix(self): + """ + Computes the joint space inertia matrix. + + Returns: + np.array[N, N]: joint space inertia matrix (where `N` is the number of DoFs). + """ + pass + + def get_inertia_inverse_times_vector(self, vector): + r""" + Computes the effect of multiplying the inverse of the joint space inertia matrix :math:`H(q)` with a vector + in linear time. + + Args: + np.array[N]: vector to be multiplied with the inverse joint space inertia matrix. + + Returns: + np.array[N]: resulting vector + """ + pass + + def get_point_acceleration(self, link_id, point): + """ + Computes the linear acceleration of a point on a link. + + Args: + link_id (int): unique link id. + point (np.array[3]): position of the point in link's local frame + + Returns: + np.array[3]: The cartesian acceleration of the point in global frame + """ + pass + + def get_link_id(self, name): + """ + Return the link id associated with the given name. + + Args: + name (str): name of the link + + Returns: + int: unique link id + """ + pass + + def get_mass(self): + """ + Return the total mass of the model. + + Returns: + float: total mass + """ + pass + + def get_floating_base_link(self): + """ + Return the floating base link. + + Returns: + int: floating base link + """ + pass + + def update(self, q=None, dq=None, ddq=None): + if q is None: + q = self.q + if dq is None: + dq = self.dq + if ddq is None: + ddq = self.ddq + rbdl.UpdateKinematics(self.model, q, dq, ddq) diff --git a/pyrobolearn/priorities/models/robot_model.py b/pyrobolearn/priorities/models/robot_model.py new file mode 100644 index 0000000..8346e72 --- /dev/null +++ b/pyrobolearn/priorities/models/robot_model.py @@ -0,0 +1,388 @@ +#!/usr/bin/env python +r"""Robot model interface used in priority tasks. + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + - [2] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 + - [3] "Rigid Body Dynamics Algorithms", Featherstone, 2008 +""" + +from pyrobolearn.priorities.models import ModelInterface +from pyrobolearn.robots import Robot + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class RobotModelInterface(ModelInterface): + r"""Robot Model interface. + + Robot model interface that accepts as input a robot that inherited from `pyrobolearn.robots.Robot`. + """ + + def __init__(self, model): + """ + Initialize the robot model interface. + + Args: + model (Robot): a robot instance. + """ + super(RobotModelInterface, self).__init__(model) + + ############## + # Properties # + ############## + + @property + def model(self): + """Return the model instance.""" + return self._model + + @model.setter + def model(self, model): + """Set the model instance.""" + if not isinstance(model, Robot): + raise TypeError("Expecting the given 'model' to be an instance of `Robot`, instead got: " + "{}".format(type(model))) + self._model = model + + # alias + @property + def robot(self): + """Return the robot instance.""" + return self._model + + @property + def num_dofs(self): + """Return the number of degrees of freedom.""" + return self.robot.num_dofs + + ########### + # Methods # + ########### + + def get_joint_positions(self): + """ + Get the joint positions. + + Returns: + np.array[N]: the joint positions. + """ + # if joint accelerations already cached in instance, return it + if 'q' in self.states: + return self.states['q'] + + # get the joint accelerations, cache it, and return it + q = self.robot.get_joint_positions() + self.states['q'] = q + return q + + def get_joint_velocities(self): + """ + Get the joint velocities. + + Returns: + np.array[N]: the joint positions. + """ + # if joint accelerations already cached in instance, return it + if 'dq' in self.states: + return self.states['dq'] + + # get the joint accelerations, cache it, and return it + dq = self.robot.get_joint_velocities() + self.states['dq'] = dq + return dq + + def get_joint_accelerations(self): + """ + Get the joint accelerations. + + Returns: + np.array[N]: the joint positions. + """ + # if joint accelerations already cached in instance, return it + if 'ddq' in self.states: + return self.states['ddq'] + + # get the joint accelerations, cache it, and return it + ddq = self.robot.get_joint_accelerations() + self.states['ddq'] = ddq + return ddq + + def get_com_position(self): + """ + Get the position of the center of mass (CoM). + + Returns: + np.array[3]: position of the center of mass + """ + return self.model.get_center_of_mass_position() + + def get_com_jacobian(self): + """ + Get the CoM Jacobian. + + Returns: + np.array[N,N]: CoM Jacobian (where N is the number of DoFs) + """ + return self.model.get_com_jacobian() + + def get_com_velocity(self): + """ + Get the linear CoM velocity. + + Returns: + np.array[3]: CoM velocity. + """ + return self.model.get_com_velocity() + + def get_com_acceleration(self): + """ + Get the linear CoM acceleration. + + Returns: + np.array[3]: CoM acceleration. + """ + pass + + def get_gravity(self): + """ + Get the gravity vector applied on the model. + + Returns: + np.array[3]: gravity vector expressed in the world frame. + """ + return self.model.simulator.gravity + + def set_gravity(self, gravity): + """ + Set the gravity vector applied on the model. + + Args: + gravity (np.array[3]): gravity vector expressed in the world frame. + """ + pass + + def get_model_ordered_joint_names(self): + """ + Get the model ordered joint names. + + Returns: + list of str: list of joint names. + """ + pass + + def get_jacobian(self, link_id, point): + r""" + Get the 6D Jacobian for a point on a link, that when multiplied with :math:`\dot{q}` gives a 6D vector that + has the angular velocity as the first three entries and the linear velocity as the last three entries. + + .. math:: v = [\omega, \dot{p}] = J(q) \dot{q} + + where :math:`J(q)` is the concatenation of the angular and linear Jacobian. + + Args: + link_id (int): unique link id. + point (np.array[3]): position of the point in link's local frame + + Returns: + np.array[6, N]: 6D Jacobian (=concatenation of the angular and linear Jacobian). + """ + pass + + def get_pose(self, link_id): + """ + Return the pose of the specified link. + + Args: + link_id (int): link id + + Returns: + np.array[7]: pose (position and quaternion expressed as [x,y,z,w]) + """ + pass + + def get_velocity_twist(self, link_id): + r""" + Compute the angular and linear velocity of a link, given by :math:`v = [\omega, \dot{p}]`. + + Args: + link_id (int): link id. + + Returns: + np.array[6]: The resulting 6D spatial velocity vector where the first three elements are the angular + velocity and the last three are the linear velocity expressed in the global world reference frame. + """ + pass + + def get_acceleration_twist(self, link_id): + r""" + Compute the angular and linear acceleration of a link, given by :math:`\dot{v} = [dot{\omega}, \ddot{p}]`. + + Args: + link_id (int): link id. + + Returns: + np.array[6]: The resulting 6D spatial acceleration vector where the first three elements are the + angular acceleration and the last three are the linear acceleration expressed in the global world + reference frame. + """ + pass + + def get_relative_acceleration_twist(self, target_link_id, base_link_id): + r""" + Compute the relative angular and linear acceleration of a target link with respect to a base link. The + acceleration is given by :math:`\dot{v} = [dot{\omega}, \ddot{p}]`. + + Returns: + np.array[6]: The resulting 6D spatial acceleration vector where the first three elements are the + angular acceleration and the last three are the linear acceleration expressed in the local frame of + the base link. + """ + pass + + def set_floating_base_pose(self, pose): + """ + Set the floating base pose. Given the desired pose (=position + orientation), the corresponding joint position + values for the 6 virtual joints (attached to the floating base) are computed. + + Args: + pose (np.array[7]): the desired pose (position and orientation given as a quaternion [x,y,z,w]) of the + floating base. + """ + pass + + def set_floating_base_velocity(self, velocity): + """ + Set the floating base velocity. This computes the corresponding joint velocity values for the 6 virtual joints + (that are attached to the floating base). + + Args: + np.array[3], np.array[6]: desired linear (and angular) velocity of the floating base. + """ + pass + + def compute_gravity_compensation(self): + """ + Return the torques to perform gravity compensation. + + Returns: + np.array[N]: torques to perform gravity compensation. + """ + pass + + def compute_nonlinear_term(self): + r""" + Computes the non-linear terms :math:`C(q, \dot{q})` in the dynamic equation of motion for a rigid-body system, + given by: + + .. math:: \tau = H(q) \ddot{q} + C(q, \dot{q}) + + where ":math:`\tau` is the vector of applied forces, :math:`H` is the joint space inertia matrix, + :math:`C(q, \dot{q})` is the vector of force terms that account for the Coriolis and centrifugal forces, + gravity, and any other forces acting on the system other than those in :math:`\tau`." [1] + + Returns: + np.array[N]: non-linear force terms. + + References: + - [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008 + """ + pass + + def compute_JdotQdot(self): + r""" + Compute :math:`\dot{J}(q) \dot{q}`, which appears in :math:`\dot{v} = J(q) \ddot{q} + \dot{J}(q) \dot{q}`, + which is the first time derivative of :math:`v = J(q) \dot{q}`. + + Returns: + np.array[6]: the matrix multiplication of the first derivative of the Jacobian with the joint velocities. + """ + pass + + def compute_relative_JdotQdot(self, target_link_id, base_link_id): + r""" + Compute the relative :math:`\dot{J}(q) \dot{q}`, which appears in + :math:`\dot{v} = J(q) \ddot{q} + \dot{J}(q) \dot{q}`, which is the first time derivative of + :math:`v = J(q) \dot{q}`. The Jacobian is taken from the specified base link to the target link. + + Args: + target_link_id (int): target link id + base_link_id (int): base link id + + Returns: + np.array[6]: relative :math:`\dot{J}(q) \dot{q}` + """ + pass + + def get_inertia_matrix(self): + """ + Computes the joint space inertia matrix. + + Returns: + np.array[N, N]: joint space inertia matrix (where `N` is the number of DoFs). + """ + pass + + def get_inertia_inverse_times_vector(self, vector): + r""" + Computes the effect of multiplying the inverse of the joint space inertia matrix :math:`H(q)` with a vector + in linear time. + + Args: + np.array[N]: vector to be multiplied with the inverse joint space inertia matrix. + + Returns: + np.array[N]: resulting vector + """ + pass + + def get_point_acceleration(self, link_id, point): + """ + Computes the linear acceleration of a point on a link. + + Args: + link_id (int): unique link id. + point (np.array[3]): position of the point in link's local frame + + Returns: + np.array[3]: The cartesian acceleration of the point in global frame + """ + pass + + def get_link_id(self, name): + """ + Return the link id associated with the given name. + + Args: + name (str): name of the link + + Returns: + int: unique link id + """ + return self.model.get_link_ids(name) + + def get_mass(self): + """ + Return the total mass of the model. + + Returns: + float: total mass + """ + return self.model.mass + + def get_floating_base_link(self): + """ + Return the floating base link. + + Returns: + int: floating base link + """ + return -1 diff --git a/pyrobolearn/priorities/solvers/README.rst b/pyrobolearn/priorities/solvers/README.rst new file mode 100644 index 0000000..1e799c0 --- /dev/null +++ b/pyrobolearn/priorities/solvers/README.rst @@ -0,0 +1,15 @@ +Solvers +======= + +In this folder, we provide the various task solvers used to solve a task or stack of tasks. + +These include: + +- ``TaskSolver``: the abstract task solver from which all the other task solvers inherit from. +- ``QPTaskSolver``: the task solver that uses quadratic programming (QP) to solve the task / stack of tasks. +- ``NLPTaskSolver``: the task solver that uses nonlinear programming (NLP) to solve the task / stack of tasks. + +References: + +1. "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" (`code `_, `slides `_, `tutorial video `_, `old code `_, LGPLv2), Rocchi et al., 2015 +2. "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 diff --git a/pyrobolearn/priorities/solvers/__init__.py b/pyrobolearn/priorities/solvers/__init__.py new file mode 100644 index 0000000..4246ce8 --- /dev/null +++ b/pyrobolearn/priorities/solvers/__init__.py @@ -0,0 +1,9 @@ + +# import abstract task solver +from .task_solver import TaskSolver + +# import QP task solver +from .qp_task_solver import QPTaskSolver + +# import Non-linear task solver +from .nlp_task_solver import NLPTaskSolver diff --git a/pyrobolearn/priorities/solvers/nlp_task_solver.py b/pyrobolearn/priorities/solvers/nlp_task_solver.py new file mode 100644 index 0000000..5a5e1f5 --- /dev/null +++ b/pyrobolearn/priorities/solvers/nlp_task_solver.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python +r"""Provide the non-linear task solver. + +Warnings: This optimization process might take time to solve the task. + +References: + - [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + - [3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 +""" + +import numpy as np + +from pyrobolearn.priorities.solvers.task_solver import TaskSolver +from pyrobolearn.optimizers.nlopt_optimizer import NLopt + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class NLPTaskSolver(TaskSolver): + r"""Nonlinear Programming Task Solver. + + The NLP task solver uses Non-Linear Programming to solve a task or stack of tasks. + + Warnings: This optimization process might take time to solve the task. + """ + + def __init__(self, task, method, submethod=None, seed=None): + """ + Initialize the task solver. + + Args: + task (Task): Priority tasks. + method (str): primary optimization method to be used. + submethod (str): sub-optimization method to be used in the primary optimization method. + seed (None, int): random seed + """ + solver = NLopt(method=method, submethod=submethod, seed=seed) + super(NLPTaskSolver, self).__init__(task, solver) + + ########### + # Methods # + ########### + + def update(self): + """Update the priority task; compute the matrices and vectors to be used later in the `solve` method.""" + self.task.update() + + def solve(self): + """Solve the priority task.""" + if self.task.tasks: + for soft_task in self.task.tasks: + As = np.vstack([np.dot(np.sqrt(task.weight), task.A) for task in soft_task]) + bs = np.vstack([np.dot(np.sqrt(task.weight), task.b) for task in soft_task]) + # x = self.solver.optimize(P=As.T.dot(As), q=-bs.T.dot(), G=, h=, A=, b=) + else: + pass diff --git a/pyrobolearn/priorities/solvers/qp_task_solver.py b/pyrobolearn/priorities/solvers/qp_task_solver.py new file mode 100644 index 0000000..ec41fc9 --- /dev/null +++ b/pyrobolearn/priorities/solvers/qp_task_solver.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python +r"""Provide the task solver that uses quadratic programming. + +References: + - [1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017 + - [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + - [3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 +""" + +import numpy as np + +from pyrobolearn.priorities.solvers.task_solver import TaskSolver +from pyrobolearn.optimizers.qpsolvers_optimizer import QP + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["OpenSoT (Enrico Mingo Hoffman and Alessio Rocchi, C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class QPTaskSolver(TaskSolver): + r"""QP Task Solver. + + The QP task solver uses QP to solve a task or stack of tasks. + """ + + def __init__(self, task, method='quadprog'): + """ + Initialize the task solver. + + Args: + task (Task): Priority tasks. + method (str): QP method/library to use. Select between ['cvxopt', 'cvxpy', 'ecos', 'gurobi', 'mosek', + 'osqp', 'qpoases', 'quadprog'] + """ + solver = QP(method=method) + super(QPTaskSolver, self).__init__(task, solver) + + ########### + # Methods # + ########### + + def update(self): + """Update the priority task; compute the matrices and vectors to be used later in the `solve` method.""" + self.task.update() + + def solve(self): + """Solve the priority task.""" + if self.task.tasks: + for soft_task in self.task.tasks: + As = np.vstack([np.dot(np.sqrt(task.weight), task.A) for task in soft_task]) + bs = np.vstack([np.dot(np.sqrt(task.weight), task.b) for task in soft_task]) + # x = self.solver.optimize(P=As.T.dot(As), q=-bs.T.dot(), G=, h=, A=, b=) + else: + pass diff --git a/pyrobolearn/priorities/solver.py b/pyrobolearn/priorities/solvers/task_solver.py similarity index 53% rename from pyrobolearn/priorities/solver.py rename to pyrobolearn/priorities/solvers/task_solver.py index c9572ca..6dcfd5c 100644 --- a/pyrobolearn/priorities/solver.py +++ b/pyrobolearn/priorities/solvers/task_solver.py @@ -1,22 +1,22 @@ #!/usr/bin/env python -r"""Provide the various task solvers which uses QP. +r"""Provide the abstract task solver class from which all the other task solvers inherit from. +A task solver accepts as inputs a task (or a stack of tasks) and an optimizer to use to solve the task. References: - [1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017 - [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 - [3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + - [2] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 """ import numpy as np from pyrobolearn.priorities.tasks.task import Task -from pyrobolearn.optimizers.qpsolvers_optimizer import QP +from pyrobolearn.optimizers import Optimizer __author__ = "Brian Delhaisse" -__copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["OpenSoT (Enrico Mingo Hoffman and Alessio Rocchi)", "Songyan Xin"] +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["OpenSoT (Enrico Mingo Hoffman and Alessio Rocchi, C++)", "Brian Delhaisse (Python + doc)"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" @@ -26,9 +26,11 @@ __status__ = "Development" class TaskSolver(object): r"""Task solver. + + The task solver accepts a task, or stack of tasks, and an optimization solver. """ - def __init__(self, task): + def __init__(self, task, solver=None): """ Initialize the task solver. @@ -36,7 +38,7 @@ class TaskSolver(object): task (Task): Priority tasks. """ self.task = task - self.solver = QP(method='qpoases') + self.solver = solver ############## # Properties # @@ -44,17 +46,30 @@ class TaskSolver(object): @property def task(self): - """Return the priority task.""" + """Return the priority task / stack of tasks.""" return self._task @task.setter def task(self, task): - """Set the priority task.""" + """Set the priority task / stack of tasks.""" if not isinstance(task, Task): raise TypeError("Expecting the given 'task' to be an instance of `Task`, instead got: " "{}".format(type(task))) self._task = task + @property + def solver(self): + """Return the optimizer/solver instance.""" + return self._solver + + @solver.setter + def solver(self, solver): + """Set the optimizer/solver instance.""" + if solver is not None and not isinstance(solver, Optimizer): + raise TypeError("Expecting the given 'solver' to be an instance of `Optimizer`, instead got: " + "{}".format(type(solver))) + self._solver = solver + ########### # Methods # ########### @@ -77,5 +92,10 @@ class TaskSolver(object): # Operators # ############# + def __str__(self): + """Return a string describing the task solver.""" + return self.__class__.__name__ + def __call__(self): + """Solve the task using the optimizer, and returned the optimized variables.""" return self.solve() diff --git a/pyrobolearn/priorities/tasks/README.md b/pyrobolearn/priorities/tasks/README.md deleted file mode 100644 index de08017..0000000 --- a/pyrobolearn/priorities/tasks/README.md +++ /dev/null @@ -1,15 +0,0 @@ -## Tasks - -In this folder, we define the most common objective functions (aka "tasks") used in robotics for priority tasks. -Several of them were provided in [1, 2]. - -Tasks include cartesian CoM tracking, cartesian end-effector position tracking, postural positioning, and others. - -References: -1. "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" ( - [code](https://opensot.wixsite.com/opensot), - [slides](https://docs.google.com/presentation/d/1kwJsAnVi_3ADtqFSTP8wq3JOGLcvDV_ypcEEjPHnCEA), - [tutorial video](https://www.youtube.com/watch?v=yFon-ZDdSyg), - [old code](https://github.com/songcheng/OpenSoT)), Rocchi et al., 2015 -2. "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 - diff --git a/pyrobolearn/priorities/tasks/README.rst b/pyrobolearn/priorities/tasks/README.rst new file mode 100644 index 0000000..a38a6f6 --- /dev/null +++ b/pyrobolearn/priorities/tasks/README.rst @@ -0,0 +1,114 @@ +Tasks +===== + +In this folder, we define the most common objective functions (aka "tasks") used in robotics for priority tasks. +Several of them were provided in [1]_, [2]_. + +The tasks presented here represents the quadratic objective functions used in quadratic programming (QP). + +A quadratic program (QP) is written in standard form [1]_ as: + +.. math:: + + x^* =& \arg \min_x \; \frac{1}{2} x^T Q x + p^T x \\ + & \text{subj. to } \; \begin{array}{c} Gx \leq h \\ Fx = c \end{array} + + +where :math:`x` is the vector being optimized (in robotics, it can be joint positions, velocities, torques, ...), +"the matrix :math:`Q` and vector :math:`p` are used to define any quadratic objective function of these variables, +while the matrix-vector couples :math:`(G,h)` and :math:`(F,c)` respectively define inequality and equality +constraints" [1]_. Inequality constraints can include the lower bounds and upper bounds of :math:`x` by setting +:math:`G` to be the identity matrix or minus this one, and :math:`h` to be the upper or minus the lower bounds. + +For instance, the quadratic objective function :math:`||Ax - b||_{W}^2` (where :math:`W` is a symmetric weight matrix) +is given in the standard form as: + +.. math:: ||Ax - b||_{W}^2 = (Ax - b)^\top W (Ax - b) = x^\top A^\top W A x - 2 b^\top W A x + b^\top W b + +where the last term :math:`b^\top W b` can be removed as it does not depend on the variables we are optimizing (i.e. +:math:`x`). We thus have :math:`Q = A^\top W A` a symmetric matrix and :math:`p = -2 A^\top W b`. + +Note that if we had instead :math:`||Ax - b||_{W}^2 + c^\top x`, this could be rewritten as: + +.. math:: ||Ax - b||_{W}^2 + c^\top x = x^\top A^\top W A x - (2 b^\top W A - c^\top) x + b^\top W b, + +giving :math:`Q = A^\top W A` and :math:`p = (c - 2 A^\top W b)`. + +Many control problems in robotics can be formulated as a quadratic programming problem. For instance, let's assume +that we want to optimize the joint velocities :math:`\dot{q}` given the end-effector's desired position and velocity +in task space. We can define the quadratic problem as: + +.. math:: || J(q) \dot{q} - v_c ||^2 + +where :math:`v_c = K_p (x_d - x) + K_d (v_d - \dot{x})` (using PD control), with :math:`x_d` and :math:`x` the desired +and current end-effector's position respectively, and :math:`v_d` is the desired velocity. The solution to this +task (i.e. optimization problem) is the same solution given by `inverse kinematics`. Now, you can even obtain the +damped least squares inverse kinematics by adding a soft task such that +:math:`||J(q)\dot{q} - v_c||^2 + ||q||^2` is optimized (note that :math:`||q||^2 = ||A q - b||^2`, where :math:`A=I` is +the identity matrix and :math:`b=0` is the zero/null vector). + + +- **Soft** priority tasks: with soft-priority tasks, the quadratic programming problem being minimized for :math:`n` + such tasks is given by: + + .. math:: + + \begin{array}{c} + x^* = \arg \min_x ||A_1 x - b_1||_{W_1}^2 + ||A_2 x - b_2 ||_{W_2}^2 + ... + ||A_n x - b_n ||_{W_n}^2 \\ + \text{subj. to } \; \begin{array}{c} Gx \leq h \\ Fx = c \end{array} + \end{array} + + Often, the weight PSD matrices :math:`W_i` are just positive scalars :math:`w_i`. This problem can notably be solved + by stacking the :math:`A_i` one of top of another, and stacking the :math:`b_i` and :math:`W_i` in the same manner, + and solving :math:`||A x - b||_{W}^2`. This is known as the augmented task. When the matrices :math:`A_i` are + Jacobians this is known as the augmented Jacobian (which can sometimes be ill-conditioned). + +- **Hard** priority tasks: with hard-priority tasks, the quadratic programming problem for :math:`n` tasks is defined + in a sequential manner, where the first most important task will be first optimized, and then the subsequent tasks + will be optimized one after the other. Thus, the first task to be optimized is given by: + + .. math:: + + x_1^* =& \arg \min_x \; ||A_1 x - b_1||^2 \\ + & \text{subj. to } \; \begin{array}{c} G_1 x \leq h_1 \\ F_1 x = c_1 \end{array} + + while the second next most important task that would be solved is given by: + + .. math:: + + x_2^* =& \arg \min_x \; ||A_2 x - b_2||^2 \\ + & \begin{array}{cc} \text{subj. to } & G_2 x \leq h_2 \\ + & F_2 x = c_2 \\ + & A_1 x = A_1 x_1^* \\ + & G_1 x \leq h_1 \\ + & F_1 x = c_1, \end{array} + + until the :math:`n` most important task, given by: + + .. math:: + + x_n^* =& \arg \min_x \; ||A_n x - b_n||^2 \\ + & \begin{array}{cc} \text{subj. to } & A_1 x = A_1 x_1^* \\ + & ... \\ + & A_{n-1} x = A_{n-1} x_{n-1}^* \\ + & G_1 x \leq h_1 \\ + & ... \\ + & G_n x \leq h_n \\ + & F_1 x = c_1 \\ + & ... \\ + & F_n x = c_n. \end{array} + + By setting the previous :math:`A_{i-1} x = A_{i-1} x_{i-1}^*` as equality constraints, the current solution + :math:`x_i^*` won't change the optimality of all higher priority tasks. + + +Tasks include cartesian CoM tracking, cartesian end-effector position tracking, postural positioning, and others. + +References: + .. [1] `Quadratic Programming in Python `_, Caron, 2017 + .. [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + (`code `_, + `slides `_, + `tutorial video `_, + `old code `_, LGPLv2) + .. [3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 diff --git a/pyrobolearn/priorities/tasks/__init__.py b/pyrobolearn/priorities/tasks/__init__.py index dcd7d27..7cb8910 100644 --- a/pyrobolearn/priorities/tasks/__init__.py +++ b/pyrobolearn/priorities/tasks/__init__.py @@ -2,8 +2,20 @@ # import task from .task import * +# import velocity tasks +from . import velocity + +# import acceleration tasks +from . import acceleration + +# import torque tasks +from . import torque + +# import force tasks +from . import force + # import kinematic tasks -from .kinematic_tasks import * +# from .kinematic_tasks import * # import dynamic tasks -from .dynamic_tasks import * +# from .dynamic_tasks import * diff --git a/pyrobolearn/priorities/tasks/acceleration/__init__.py b/pyrobolearn/priorities/tasks/acceleration/__init__.py new file mode 100644 index 0000000..6409d0b --- /dev/null +++ b/pyrobolearn/priorities/tasks/acceleration/__init__.py @@ -0,0 +1,8 @@ + +from .cartesian import CartesianAccelerationTask + +from .com import CoMAccelerationTask + +from .contact import ContactAccelerationTask + +from .postural import PosturalAccelerationTask diff --git a/pyrobolearn/priorities/tasks/acceleration/cartesian.py b/pyrobolearn/priorities/tasks/acceleration/cartesian.py new file mode 100644 index 0000000..93433b2 --- /dev/null +++ b/pyrobolearn/priorities/tasks/acceleration/cartesian.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python +r"""Provide the Cartesian acceleration task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +# TODO: finish to implement this + +import numpy as np + +from pyrobolearn.priorities.tasks import JointAccelerationTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Arturo Laurenzi (C++)", "Songyan Xin (insight)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class CartesianAccelerationTask(JointAccelerationTask): + r"""Cartesian Acceleration Task + + The Cartesian acceleration task tries to impose a desired pose, velocity and acceleration profiles for a distal + link with respect to a base link, or world frame. + + Before presenting the optimization problem, a small reminder. The acceleration is the time derivative of the + velocity, i.e. :math:`a = \frac{dv}{dt}` where the cartesian velocities are related to joint velocities by + :math:`v = J(q) \dot{q}` where :math:`J(q)` is the Jacobian, thus deriving that expression wrt time gives us: + + .. math:: a = \frac{d}{dt} v = \frac{d}{dt} J(q) \dot{q} = J(q) \ddot{q} + \dot{J}(q) \dot{q}. + + Now, we can formulate our minimization problem as: + + .. math:: || J(q) \ddot{q} - \dot{J} \dot{q} - (a_d + K_d (v_d - v) + K_p e) ||^2, + + where :math:`\ddot{q}` are the joint accelerations being optimized, :math:`a_d` is the desired cartesian + acceleration, :math:`v_d = [\omega_d^\top, v` is the desired cartesian velocity, ... + + + The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting + :math:`A = J(q)`, :math:`x = \ddot{q}`, and :math:`b = - \dot{J} \dot{q} - (a_d + K_d (v_d - v) + K_p e)`. + + This task can, for instance, be used for foot pose tracking when this one is not in contact with the ground. If + the foot is in contact, we switch to a foot damping task which can be achieved by setting + :math:`a_d = v_d = e = 0` and thus we are trying to solve :math:`||J(q) \ddot{q} - \dot{J} \dot{q} - K_d v_d||^2`. + + + Inverse dynamics + ---------------- + + Once the optimal joint accelerations :math:`\ddot{q}^*` have been computed, we can use inverse dynamics to + compute the corresponding torques to apply on the joints. This is given by: + + .. math:: \tau = H(q) \ddot{q} + N(q,\dot{q)} + + where :math:`H(q)` is the inertia joint matrix, and N(q, \dot{q}) is a vector force that accounts for all the + other forces acting on the system (Coriolis, centrifugal, gravity, external forces, friction, etc.). + + + .. seealso:: `tasks/velocity/cartesian.py` and `tasks/torque/cartesian_impedance_control.py` + """ + + def __init__(self, model, distal_link, base_link=None, local_position=(0, 0, 0), x_desired=None, + dx_desired=None, kp=1., kd=1., weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + distal_link (int, str): distal link id or name. + base_link (int, str, None): base link id or name. If None, it will be the world. + local_position (np.array[3]): local position on the distal link. + x_desired (np.array[7], None): desired cartesian pose of distal link wrt the base. + dx_desired (np.array[6], None): desired cartesian velocity of distal link wrt the base. + kp (float, np.array[6,6]): stiffness gain. + weight (float, np.array[6,6]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(CartesianAccelerationTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # set variables + self.distal_link = distal_link + self.base_link = base_link + + # pose = (position, quaternion) + self.desired_pose = np.array([0]*6 + 1) + self.current_pose = np.array([0]*6 + 1) + + # velocity = (angular, linear) + self.desired_velocity = np.zeros(6) + self.current_velocity = np.zeros(6) + + # acceleration = (angular, linear) + self.desired_acceleration = np.zeros(6) diff --git a/pyrobolearn/priorities/tasks/acceleration/com.py b/pyrobolearn/priorities/tasks/acceleration/com.py new file mode 100644 index 0000000..8f5c7e9 --- /dev/null +++ b/pyrobolearn/priorities/tasks/acceleration/com.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python +r"""Provide the Cartesian CoM acceleration task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import Task + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Songyan Xin (insight)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class CoMAccelerationTask(Task): + r"""CoM Acceleration Task + + """ + + def __init__(self, model, constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface + constraints (list of Constraint): list of constraints associated with the task. + """ + super(CoMAccelerationTask, self).__init__(model=model, constraints=constraints) diff --git a/pyrobolearn/priorities/tasks/acceleration/contact.py b/pyrobolearn/priorities/tasks/acceleration/contact.py new file mode 100644 index 0000000..7dda6c9 --- /dev/null +++ b/pyrobolearn/priorities/tasks/acceleration/contact.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python +r"""Provide the contact acceleration task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import Task + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Arturo Laurenzi (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class ContactAccelerationTask(Task): + r"""Contact Acceleration Task + + """ + + def __init__(self, model, constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface + constraints (list of Constraint): list of constraints associated with the task. + """ + super(ContactAccelerationTask, self).__init__(model=model, constraints=constraints) diff --git a/pyrobolearn/priorities/tasks/acceleration/postural.py b/pyrobolearn/priorities/tasks/acceleration/postural.py new file mode 100644 index 0000000..5a60f15 --- /dev/null +++ b/pyrobolearn/priorities/tasks/acceleration/postural.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python +r"""Provide the postural acceleration task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import Task + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Arturo Laurenzi (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class PosturalAccelerationTask(Task): + r"""Postural Acceleration Task + + """ + + def __init__(self, model, constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface + constraints (list of Constraint): list of constraints associated with the task. + """ + super(PosturalAccelerationTask, self).__init__(model=model, constraints=constraints) diff --git a/pyrobolearn/priorities/tasks/force/__init__.py b/pyrobolearn/priorities/tasks/force/__init__.py new file mode 100644 index 0000000..12d0ca4 --- /dev/null +++ b/pyrobolearn/priorities/tasks/force/__init__.py @@ -0,0 +1,6 @@ + +from .com import CoMForceTask + +from .floating_base import FloatingBaseForceTask + +from .wrench import WrenchTask diff --git a/pyrobolearn/priorities/tasks/force/com.py b/pyrobolearn/priorities/tasks/force/com.py new file mode 100644 index 0000000..968c3fe --- /dev/null +++ b/pyrobolearn/priorities/tasks/force/com.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python +r"""Provide the center of mass force task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import Task + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Alessio Rocchi (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class CoMForceTask(Task): + r"""CoM Force Task + + """ + + def __init__(self, model, constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface + constraints (list of Constraint): list of constraints associated with the task. + """ + super(CoMForceTask, self).__init__(model=model, constraints=constraints) diff --git a/pyrobolearn/priorities/tasks/force/floating_base.py b/pyrobolearn/priorities/tasks/force/floating_base.py new file mode 100644 index 0000000..0da4ccd --- /dev/null +++ b/pyrobolearn/priorities/tasks/force/floating_base.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python +r"""Provide the floating base force task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import Task + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Arturo Laurenzi (C++)", "Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class FloatingBaseForceTask(Task): + r"""Floating base Force Task + + """ + + def __init__(self, model, constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface + constraints (list of Constraint): list of constraints associated with the task. + """ + super(FloatingBaseForceTask, self).__init__(model=model, constraints=constraints) diff --git a/pyrobolearn/priorities/tasks/force/manipulability.py b/pyrobolearn/priorities/tasks/force/manipulability.py new file mode 100644 index 0000000..01a66ea --- /dev/null +++ b/pyrobolearn/priorities/tasks/force/manipulability.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python +r"""Provide the force manipulability task. + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import Task + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class ForceManipulabilityTask(Task): + r"""Force Manipulability Task + + The manipulability task implements a tasks that tries to maximize the force manipulability measure given in [1]: + + .. math:: w(q) = \sqrt( \det( (J(q) W J(q)^\top)^{-1} ) ) + + where :math:`W` is a constant weight matrix, :math:`q` are the joint positions, and :math:`J(q)` is the jacobian. + The gradient of :math:`w` is then computed and projected using the gradient projection method [2]. + + References: + - [1] "Robotics: Modelling, Planning, and Control", Siciliano et al., 2010 + - [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + """ + + def __init__(self, model, constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(ForceManipulabilityTask, self).__init__(model=model, constraints=constraints) diff --git a/pyrobolearn/priorities/tasks/force/wrench.py b/pyrobolearn/priorities/tasks/force/wrench.py new file mode 100644 index 0000000..85469d0 --- /dev/null +++ b/pyrobolearn/priorities/tasks/force/wrench.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python +r"""Provide the wrench task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import Task + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class WrenchTask(Task): + r"""Wrench Task + + """ + + def __init__(self, model, constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface + constraints (list of Constraint): list of constraints associated with the task. + """ + super(WrenchTask, self).__init__(model=model, constraints=constraints) diff --git a/pyrobolearn/priorities/tasks/task.py b/pyrobolearn/priorities/tasks/task.py index 5aaf791..2b6347a 100644 --- a/pyrobolearn/priorities/tasks/task.py +++ b/pyrobolearn/priorities/tasks/task.py @@ -3,74 +3,89 @@ r"""Provide the various tasks (i.e. objective functions) used in QP. The tasks presented here represents the quadratic objective functions used in quadratic programming (QP). -A quadratic program (QP) is written in standard form [1] as: +A quadratic program (QP) is written in standard form [1]_ as: .. math:: - x^* &= \arg \min_x \frac{1}{2} x^T Q x + p^T x \\ \text{subj. to} - & Gx \leq h \\ - & Fx = c + x^* =& \arg \min_x \; \frac{1}{2} x^T Q x + p^T x \\ + & \text{subj. to } \; \begin{array}{c} Gx \leq h \\ Fx = c \end{array} + where :math:`x` is the vector being optimized (in robotics, it can be joint positions, velocities, torques, ...), "the matrix :math:`Q` and vector :math:`p` are used to define any quadratic objective function of these variables, while the matrix-vector couples :math:`(G,h)` and :math:`(F,c)` respectively define inequality and equality -constraints" [1]. Inequality constraints can include the lower bounds and upper bounds of :math`x` by setting -:math:`G` to be the identity matrix or minus this one, and :math:`h` to be the upper or lower bounds. +constraints" [1]_. Inequality constraints can include the lower bounds and upper bounds of :math:`x` by setting +:math:`G` to be the identity matrix or minus this one, and :math:`h` to be the upper or minus the lower bounds. -For instance, the quadratic objective function :math:`||Ax - b||^2_{W}` (where :math:`W` is a weight matrix) is given -in the standard form as: +For instance, the quadratic objective function :math:`||Ax - b||_{W}^2` (where :math:`W` is a symmetric weight matrix) +is given in the standard form as: -.. math:: ||Ax - b||^2_{W} = (Ax - b)^\top W (Ax - b) = x^\top A^\top W A x - 2 b^\top W A x + b^\top W b +.. math:: ||Ax - b||_{W}^2 = (Ax - b)^\top W (Ax - b) = x^\top A^\top W A x - 2 b^\top W A x + b^\top W b where the last term :math:`b^\top W b` can be removed as it does not depend on the variables we are optimizing (i.e. -:math:`x`). We thus have :math:`Q = A^\top W A` a symmetric matrix and :math:`p = -2 b^\top W A`. +:math:`x`). We thus have :math:`Q = A^\top W A` a symmetric matrix and :math:`p = -2 A^\top W b`. -Many control problems in robotics can be formulated as a quadratic programming problem. +Note that if we had instead :math:`||Ax - b||_{W}^2 + c^\top x`, this could be rewritten as: -For instance, let's assume that we want to optimize the joint velocities :math:`\dot{q}` given the end-effector's -desired position and velocity in task space. We can define the quadratic problem as: +.. math:: ||Ax - b||_{W}^2 + c^\top x = x^\top A^\top W A x - (2 b^\top W A - c^\top) x + b^\top W b, -.. math:: || J(q) \dot{q} - \dot{x} ) ||^2 +giving :math:`Q = A^\top W A` and :math:`p = (c - 2 A^\top W b)`. -where using a PD reference, :math:`\dot{x} = \dot{x}_d + K (x_d - x)`, where :math:`x_d` and :math:`x` are the desired -and current end-effector's position respectively, and :math:`\dot{x}_d` is the desired velocity. +Many control problems in robotics can be formulated as a quadratic programming problem. For instance, let's assume +that we want to optimize the joint velocities :math:`\dot{q}` given the end-effector's desired position and velocity +in task space. We can define the quadratic problem as: + +.. math:: || J(q) \dot{q} - v_c ||^2 + +where :math:`v_c = K_p (x_d - x) + K_d (v_d - \dot{x})` (using PD control), with :math:`x_d` and :math:`x` the desired +and current end-effector's position respectively, and :math:`v_d` is the desired velocity. The solution to this +task (i.e. optimization problem) is the same solution given by `inverse kinematics`. Now, you can even obtain the +damped least squares inverse kinematics by adding a soft task such that +:math:`||J(q)\dot{q} - v_c||^2 + ||q||^2` is optimized (note that :math:`||q||^2 = ||A q - b||^2`, where :math:`A=I` is +the identity matrix and :math:`b=0` is the zero/null vector). -* Soft priority tasks: with soft-priority tasks, the quadratic programming problem being minimized for n such tasks -is given by: +- **Soft** priority tasks: with soft-priority tasks, the quadratic programming problem being minimized for :math:`n` + such tasks is given by: -.. math:: + .. math:: - x^* &= \arg \min_x ||A_1 x - b_1||^2_{W_1} + ||A_2 x - b_2 ||^2_{W_2} + ... + ||A_n x - b_n ||^2_{W_n} \\ - \text{subj. to} & Gx \leq h \\ - & Fx = c + \begin{array}{c} + x^* = \arg \min_x ||A_1 x - b_1||_{W_1}^2 + ||A_2 x - b_2 ||_{W_2}^2 + ... + ||A_n x - b_n ||_{W_n}^2 \\ + \text{subj. to } \; \begin{array}{c} Gx \leq h \\ Fx = c \end{array} + \end{array} -Often, the weight matrices :math:`W_i` are just scalars :math:`w_i`. This problem can notably be solved by stacking -the :math:`A_i` one of top of another, and stacking the :math:`b_i` and :math:`W_i` in the same manner, and solving -:math:`||A x - b||^2_{W}` This is known as the augmented task. When the matrices :math:`A` are Jacobians this is known -as the augmented Jacobian (which can sometimes be ill-conditioned). + Often, the weight PSD matrices :math:`W_i` are just positive scalars :math:`w_i`. This problem can notably be solved + by stacking the :math:`A_i` one of top of another, and stacking the :math:`b_i` and :math:`W_i` in the same manner, + and solving :math:`||A x - b||_{W}^2`. This is known as the augmented task. When the matrices :math:`A_i` are + Jacobians this is known as the augmented Jacobian (which can sometimes be ill-conditioned). -* Hard priority tasks: with hard-priority tasks, the quadratic programming problem for n tasks is defined in a -sequential manner, where the first most important task will be first optimized, and then the subsequent tasks will be -optimized one after the other. Thus, the first task to be optimized is given by: +- **Hard** priority tasks: with hard-priority tasks, the quadratic programming problem for :math:`n` tasks is defined + in a sequential manner, where the first most important task will be first optimized, and then the subsequent tasks + will be optimized one after the other. Thus, the first task to be optimized is given by: -.. math:: x_1^* &= \arg \min_x ||A_1 x - b_1||^2 \\ \text{subj. to} - & G_1 x \leq h_1 \\ - & F_1 x = c_1, + .. math:: -while the second next most important task that would be solved is given by: + x_1^* =& \arg \min_x \; ||A_1 x - b_1||^2 \\ + & \text{subj. to } \; \begin{array}{c} G_1 x \leq h_1 \\ F_1 x = c_1 \end{array} -.. math:: x_2^* &= \arg \min_x ||A_2 x - b_2||^2 \\ \text{subj. to} - & G_2 x \leq h_2 \\ + while the second next most important task that would be solved is given by: + + .. math:: + + x_2^* =& \arg \min_x \; ||A_2 x - b_2||^2 \\ + & \begin{array}{cc} \text{subj. to } & G_2 x \leq h_2 \\ & F_2 x = c_2 \\ & A_1 x = A_1 x_1^* \\ & G_1 x \leq h_1 \\ - & F_1 x = c_1, + & F_1 x = c_1, \end{array} -until the :math:`n` most important task, given by: + until the :math:`n` most important task, given by: -.. math:: x_n^* \arg \min_x ||A_n x - b_n||^2 \\ \text{subj. to} - & A_1 x = A_1 x_1^* \\ + .. math:: + + x_n^* =& \arg \min_x \; ||A_n x - b_n||^2 \\ + & \begin{array}{cc} \text{subj. to } & A_1 x = A_1 x_1^* \\ & ... \\ & A_{n-1} x = A_{n-1} x_{n-1}^* \\ & G_1 x \leq h_1 \\ @@ -78,25 +93,28 @@ until the :math:`n` most important task, given by: & G_n x \leq h_n \\ & F_1 x = c_1 \\ & ... \\ - & F_n x = c_n. + & F_n x = c_n. \end{array} -By setting the previous :math:`A_{i-1} x = A_{i-1} x_{i-1}^*` as equality constraints, the current solution -:math:`x_i^*` won't change the optimality of all higher priority tasks. + By setting the previous :math:`A_{i-1} x = A_{i-1} x_{i-1}^*` as equality constraints, the current solution + :math:`x_i^*` won't change the optimality of all higher priority tasks. +The implementation of this class and the subsequent classes is inspired by [2] (which is licensed under the LGPLv2). References: - [1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017 - [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 - [3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 + - [1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017 + - [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + - [3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017 """ import numpy as np +from pyrobolearn.priorities.models import ModelInterface from pyrobolearn.priorities.constraints.constraint import Constraint + __author__ = "Brian Delhaisse" -__copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["OpenSoT (Enrico Mingo Hoffman and Alessio Rocchi)", "Songyan Xin"] +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Alessio Rocchi (C++)", "Brian Delhaisse (Python + doc)"] __license__ = "GNU GPLv3" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" @@ -106,36 +124,53 @@ __status__ = "Development" # TODO: take into account constraints # TODO: take into account hard priority tasks + class Task(object): r"""Task (abstract) class. - Python implementation of Tasks based on the slides of the OpenSoT framework [1]. + This class describes the Task or Stack of Tasks (SoT). + + Python implementation of Tasks based on the OpenSoT framework [1]. References: - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" - ([code](https://opensot.wixsite.com/opensot), - [slides](https://docs.google.com/presentation/d/1kwJsAnVi_3ADtqFSTP8wq3JOGLcvDV_ypcEEjPHnCEA), - [tutorial video](https://www.youtube.com/watch?v=yFon-ZDdSyg), - [old code](https://github.com/songcheng/OpenSoT)), Rocchi et al., 2015 + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" + `code `_, + `slides `_, + `tutorial video `_, + `old code `_), Rocchi et al., 2015 """ - def __init__(self, tasks=[], model=None, weight=1., constraints=[]): + def __init__(self, stack_of_tasks=[], model=None, weight=1., constraints=[]): """ - Initialize the task. + Initialize the task, or stack of tasks. Args: - tasks (list of list of Task): list of list of tasks, where the list is ordered by hard priorities, and the - nested list contains tasks which - model (Robot, str): robot model. If str, it needs to be the path to the URDF. - constraints (list of Constraint): list of constraints. + stack_of_tasks (list of list of Task, empty list): stack of tasks represented as list of list of tasks, + where the list is ordered by hard priorities, and the nested list contains soft priority tasks that + have to be weighted together. + model (ModelInterface, None): model interface associated to the task. + weight (float, np.array[N,N]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated to the task. """ - self._tasks = tasks - self._model = model + # set and check each given parameter + self.tasks = stack_of_tasks + self.model = model self.weight = weight - self._constraints = [] + self.constraints = constraints - self._A = None - self._b = None + # check that the task is a valid task or a stack o tasks + if self.model is None and not self.is_stack_of_tasks(): + raise RuntimeError("Expecting the task to be a valid task or a stack of tasks. You can not instantiate " + "an empty task.") + + # set the number of variables to optimize + self._x_size = self.model.num_dofs + + # define task matrix and vector + if self.model is not None and not self.is_stack_of_tasks(): + self._A = np.identity(self.x_size) # None + self._b = np.zeros(self.x_size) # None + self._c = np.zeros(self.x_size) # None ############## # Properties # @@ -146,19 +181,47 @@ class Task(object): """Return the tasks.""" return self._tasks - @property - def level(self): - """Return the level of the tree.""" - if self._tasks: - return len(self._tasks) - else: - return 1 + @tasks.setter + def tasks(self, tasks): + """Set the stack of tasks.""" + # check type + if tasks is None: + tasks = [] + if not isinstance(tasks, (list, tuple, set)): + raise TypeError("Expecting the given 'tasks', to be a list of list of `Task`, instead got: " + "{}".format(type(tasks))) + + # go through the stack of tasks + for i, hard_task in enumerate(tasks): + + # if the hard task is a list of soft tasks + if isinstance(hard_task, (list, tuple, set)): + # go through each soft task in the hard task + for j, soft_task in enumerate(hard_task): + if not isinstance(soft_task, Task): + raise TypeError("The given task positioned at ({}, {}) is not an instance of `Task`, but: " + "{}".format(i, j, type(soft_task))) + else: # if not, check that the hard task is an instance of Task + if not isinstance(hard_task, Task): + raise TypeError("Expecting the {}th hard task to be an instance of `Task` or a list of `Task`, " + "instead got: {}".format(i, type(hard_task))) + + # set the stack of tasks + self._tasks = tasks @property def model(self): - """Return the robot model.""" + """Return the model interface.""" return self._model + @model.setter + def model(self, model): + """Set the model interface.""" + if model is not None and not isinstance(model, ModelInterface): + raise TypeError("Expecting the given 'model' to be None or an instance of `ModelInterface`, instead got: " + "{}".format(type(model))) + self._model = model + @property def weight(self): """Return the relative weight (used for soft priorities).""" @@ -166,36 +229,219 @@ class Task(object): @weight.setter def weight(self, weight): - if not isinstance(weight, (int, float)): - raise TypeError("Expecting the relative weight to be an int or float, instead got: " + """Set the weight scalar or matrix.""" + # check the type + if not isinstance(weight, (float, int, np.ndarray)): + raise TypeError("Expecting the given 'weight' to be an int, float, or np.ndarray, instead got: " "{}".format(type(weight))) - if weight < 0: - raise ValueError("Expecting the relative weight to be positive.") + + # if weight is a matrix, check that it is PSD + if isinstance(weight, np.ndarray): + if not self._is_positive_semidefinite(weight): + raise ValueError("Expecting the relative weight matrix to be positive semidefinite (PSD).") + + # if weight is a scalar, check it is positive + else: + if weight < 0: + raise ValueError("Expecting the relative weight to be positive.") + + # set the weight self._weight = weight + @property + def depth(self): + """Return the depth of the stack of tasks.""" + return len(self.tasks) + @property def constraints(self): """Return the constraints.""" return self._constraints + @constraints.setter + def constraints(self, constraints): + """Set the constraints.""" + # check the type + if constraints is None: + constraints = [] + if not isinstance(constraints, (list, tuple, set)): + constraints = [constraints] + + # go through each constraint and check its type + for i, constraint in enumerate(constraints): + if not isinstance(constraint, Constraint): + raise TypeError("The {}th given constraint is not an instance of `Constraint`, instead got: " + "{}".format(i, type(constraint))) + + # set the constraints associated with the task + self._constraints = constraints + + @property + def x_size(self): + """Return the number of variables being optimized.""" + return self._x_size + + @property + def num_tasks(self): + """Return the total number of tasks defined.""" + return self.get_num_tasks() + + @property + def num_hard_tasks(self): + """Return the number of hard tasks. This counts the task itself if the instance is not a stack of tasks.""" + return self.get_num_hard_tasks() + @property def A(self): - """Return A matrix used in QP.""" + r"""Return the A matrix from :math:`||Ax - b||^2` used in QP. + + Returns: + """ + if self.is_stack_of_tasks(): + pass return self._A @property def b(self): - """Return b vector used in QP.""" + r"""Return the b vector from :math:`||Ax - b||^2` used in QP.""" + if self.is_stack_of_tasks(): + pass return self._b + @property + def c(self): + r"""Return the c vector from :math:`||Ax - b||^2 + c^\top x` used in QP.""" + if self.is_stack_of_tasks(): + pass + return self._c + + @property + def Q(self): + r"""Return the Q matrix :math:`Q = A^\top W A` used in :math:`\frac{1}{2} x^T Q x + p^T x` for QP.""" + if self.is_stack_of_tasks(): + pass + return self._A.T.dot(self.weight).dot(self._A) + + @property + def p(self): + r"""Return the p vector :math:`p = (c - 2 A^\top W b)` used in :math:`\frac{1}{2} x^T Q x + p^T x` + for QP.""" + if self.is_stack_of_tasks(): + pass + return self.c - self._A.T.dot(self.weight).dot(self._b) + + ################## + # Static Methods # + ################## + + @staticmethod + def _is_positive_semidefinite(x, tol=1e-8): + """Check if the given argument is a PSD matrix. + + Args: + x (np.array): matrix to check if it a PSD matrix. + tol (float): tolerance. + """ + return np.all(np.linalg.eigvals(x) >= tol) + + def _check_weight_matrix_shape(self, shape): + """ + Check if the weight matrix has the correct shape. + + Args: + shape (tuple of int): shape that the weight matrix should have. + + Raises: + ValueError: if the shape is not the correct one. + """ + if isinstance(self.weight, np.ndarray) and self.weight.shape != shape: + raise ValueError("Expecting the given weight matrix to have a shape of {}, but instead got a shape of: " + "{}".format(shape, self.weight.shape)) + ########### # Methods # ########### + def is_stack_of_tasks(self): + """Check if the task is a stack of tasks. This returns True even if there is one task in the stack of tasks.""" + return len(self.tasks[0]) > 0 + + def is_soft_task(self): + """Check if the task is a soft task.""" + return len(self.tasks) == 1 and len(self.tasks[0]) > 1 + + def get_num_tasks(self): + """Return the total number of tasks.""" + # create counter + cnt = 0 + + # go through the stack of tasks and increment the counter for each encountered task + for hard_task in self.tasks: + for soft_task in hard_task: + cnt += 1 + + # if there was nothing in the stack of tasks, set counter to 1 (because the instance is then a task) + if cnt == 0: + cnt = 1 + + return cnt + + def get_num_hard_tasks(self): + """ + Return the number of hard tasks (i.e. the number of levels) in the stack of tasks. This counts the task itself + if the instance is not a stack of tasks. + + Returns: + int: number of hard tasks (between 1 and `len(self.tasks)`) + """ + return len(self.tasks) + + def get_num_soft_tasks(self, level): + """ + Return the number of soft tasks there are at the specified level in the stack of tasks. + + Args: + level (int): level in the stack of tasks which is in [0, ..., len(self.tasks)]. + + Returns: + int: the number of soft tasks + """ + return len(self.tasks[level]) + + def get_soft_tasks(self, hard_task_idx=0, soft_task_idx=None): + r""" + Return the specified soft tasks. + + Args: + hard_task_idx (int): level in the stack of tasks which is in [0, ..., len(self.tasks)] + soft_task_idx (int): soft task index in `self.tasks[hard_task_idx]`. If None, it will return all the soft + tasks present at the specified level `hard_task_idx`. + + Returns: + Task, list of Task: the specified soft tasks. + """ + if soft_task_idx is None: + return self.tasks[hard_task_idx] + else: + return self.tasks[hard_task_idx][soft_task_idx] + + def loss(self, x): + """ + Compute the error loss of the given task. + + Args: + x (np.array): joint velocities that are being optimized. + + Returns: + float: loss value + """ + return np.sum((self._A.dot(x) - self._b) ** 2) + def _update(self): """Update the task. - Compute the A matrix and b vector that will be used by the task solver. + Compute the A matrix and b vector that will be used by the task solver. This has to be implemented in the + child classes. Returns: np.array: A matrix used in QP. @@ -204,6 +450,14 @@ class Task(object): pass def update(self): + """ + Compute the A matrix and b vector that will be used by the task solver. + + Returns: + np.array: A matrix used in QP. + np.array: b vector used in QP. + """ + # if stack of tasks if self.tasks: for hard_task in self.tasks: results = [soft_task.update() for soft_task in hard_task] @@ -211,14 +465,22 @@ class Task(object): bs = np.vstack([result[1] for result in results]) # TODO: continue for hard priority tasks return As, bs + + # if normal task + # update the constraints + return self._update() ############# # Operators # ############# - def __repr__(self): - """Return a string representing the class.""" + # def __repr__(self): + # """Return a string representing the class.""" + # return self.__str__() + + def __str__(self): + """Return a string describing the class.""" if self.tasks: tasks = [] for i, soft_tasks in enumerate(self.tasks): @@ -233,25 +495,37 @@ class Task(object): return '\n'.join(tasks) return self.__class__.__name__ - def __str__(self): - """Return a string describing the class.""" - return self.__repr__() - def __call__(self): + """Update the tasks.""" return self.update() def __add__(self, other): # TODO: check when other has some tasks - """Add a soft priority task.""" + """Add a soft priority task. + + Examples: + task1 = Task(weight=2) + task2 = Task(weight=3) + task = task1 + task2 + print(task) + """ if not isinstance(other, Task): raise TypeError("Expecting 'other' to be an instance of Task, instead got: {}".format(type(other))) if len(self.tasks) > 0: tasks = list(self.tasks) tasks[-1].append(other) - return Task(tasks=tasks) - return Task(tasks=[[self, other]]) + return Task(stack_of_tasks=tasks) + return Task(stack_of_tasks=[[self, other]]) def __div__(self, other): - """Add a hard priority task.""" + """Append a hard priority task to the stack of tasks. + + Examples: + task1 = Task(weight=2) + task2 = Task(weight=3) + task = task1 / task2 + print(task) + """ + # check type of if not isinstance(other, Task): raise TypeError("Expecting 'other' to be an instance of Task, instead got: {}".format(type(other))) tasks = list(self.tasks) @@ -261,39 +535,114 @@ class Task(object): tasks.append(task) else: tasks.append([other]) - return Task(tasks=tasks) + return Task(stack_of_tasks=tasks) def __lshift__(self, other): - """Insert a constraint (in-place operation).""" + """Insert a constraint (in-place operation). + + Examples: + task1 = Task() + task2 = Task() + task = task1 + task2 + constraint = Constraint() + + task << constraint + print(task1.constraint) + print(task2.constraint) + """ + # check other type; it must be a constraint if not isinstance(other, Constraint): raise TypeError("Expecting 'other' to be an instance of Constraint, instead got: {}".format(type(other))) - self._constraints.append(other) + + # if we have a stack of tasks, insert the constraint for all tasks + if self.tasks: + for hard_task in self.tasks: + if isinstance(hard_task, list): + for soft_task in hard_task: + soft_task << other + else: + hard_task << other + + # if we have one task, append the constraint + else: + self.constraints.append(other) def __mul__(self, other): - """Multiply the task by a relative weight.""" - if not isinstance(other, (int, float)) or other < 0: - raise TypeError("Expecting a positive integer or float for the weight.") + """Multiply the task by a relative weight scalar or matrix.""" self.weight = other def __rmul__(self, other): - self.__mul__(other) + """Multiply the task by a relative weight""" + return self.__mul__(other) def __getitem__(self, key): """Get the corresponding task. + Args: + key (int, slice, tuple of int): key. + Examples: - >>> task1 = Task(weight=1) - >>> task2 = Task(weight=2) - >>> task = Task(tasks=[[task1, task2], [task1]]) - >>> task2 == task[0,1] # get the second priority task in the first hard task, i.e. it will return task2 - True + # >>> task1 = Task(weight=1) + # >>> task2 = Task(weight=2) + # >>> task = Task(stack_of_tasks=[[task1, task2], [task1]]) + # >>> task2 == task[0,1] # get the second priority task in the first hard task, i.e. it will return task2 + # True """ - pass + if isinstance(key, tuple) and len(key) == 2: + return self.tasks[key[0]][key[1]] + return self.tasks[key] + + +class KinematicTask(Task): + r"""Kinematic Task + + Kinematic tasks focus on tasks that optimize joint velocities. + """ + pass + + +class JointVelocityTask(Task): + r"""Joint Velocity Task + + Joint velocity tasks are tasks that optimize joint velocities :math:`\dot{q}`. + """ + pass + + +class DynamicTask(Task): + r"""Dynamic Task + + Dynamic tasks focus on tasks that involve accelerations and forces / torques. + """ + pass + + +class JointAccelerationTask(Task): + r"""Joint Acceleration Task + + Joint acceleration tasks are tasks that optimize joint accelerations :math:`\ddot{q}`. + """ + pass + + +class JointTorqueTask(Task): + r"""Joint Torque Task + + Joint torque tasks are tasks that optimize joint torques :math:`\tau`. + """ + pass # Tests if __name__ == '__main__': task1 = Task(weight=2) task2 = Task(weight=3) - task = Task(tasks=[[task1, task2], [task1]]) + task = Task(stack_of_tasks=[[task1, task2], [task1]]) + print(task) + + print(task2 == task[0, 1]) + + task = 1./2 * task1 + 1./3 * task2 + task = task / task1 + print(task) diff --git a/pyrobolearn/priorities/tasks/torque/__init__.py b/pyrobolearn/priorities/tasks/torque/__init__.py new file mode 100644 index 0000000..861f482 --- /dev/null +++ b/pyrobolearn/priorities/tasks/torque/__init__.py @@ -0,0 +1,4 @@ + +from .cartesian_impedance_control import CartesianImpedanceControlTask + +from .joint_impedance_control import JointImpedanceControlTask diff --git a/pyrobolearn/priorities/tasks/torque/cartesian_impedance_control.py b/pyrobolearn/priorities/tasks/torque/cartesian_impedance_control.py new file mode 100644 index 0000000..451c307 --- /dev/null +++ b/pyrobolearn/priorities/tasks/torque/cartesian_impedance_control.py @@ -0,0 +1,206 @@ +#!/usr/bin/env python +r"""Provide the cartesian impedance control task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import JointTorqueTask +from pyrobolearn.utils.transformation import quaternion_error + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Alessio Rocchi (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class CartesianImpedanceControlTask(JointTorqueTask): + r"""Cartesian Impedance Control Task + + The cartesian impedance control task optimizes the joint torques such that it applies the necessary torques to + move a distal link with respect to a bas e + + .. math:: || J(q) H(q)^{-1} \tau - J(q) H(q)^{-1} J(q)^\top f ||^2 = || J(q) H(q)^{-1} (\tau - J(q)^\top f) ||^2 + + where :math:`J(q) \in \mathbb{R}^{6 \times N}` is the Jacobian matrix, :math:`H(q) \in \mathbb{R}^{N \times N}` is + the joint inertia matrix, :math:`\tau \in \mathbb{R}^N` are the torques being optimized, and + :math:`f \in \mathbb{R}^6` is the desired wrench computed from: + + .. math:: f = K_p e + K_d (\dot{x}_d - \dot{x}) + + where :math:`K_p` and :math:`K_d` are the stiffness and damping gains, :math:`e \in \mathbb{R}^{6}` is the error + which is the concatenation of the position error given by :math:`e_{p} = (x_d - x)` (with :math:`x_d` being the + desired pose, and :math:`x` the current pose), and the orientation error given by (if expressed as quaternions + :math:`o = {s, v}` where :math:`s` is the real scalar part, and :math:`v` is the vector part) + :math:`e_{o} = s v_d - s_d v - v_d \cross v`, and :math:`\dot{x}_d \in \mathbb{R}^{6}` is the desired cartesian + velocity for the distal link with respect to the base link. + + The above optimization problem is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting + :math:`A = J(q) H(q)^{-1}`, :math:`x = \tau`, and :math:`b = J(q) H(q)^{-1} J(q)^\top f`. + + Note that :math:`||J(q) H(q)^{-1} (\tau - J(q)^\top f)||^2 \leq ||J(q) H(q)^{-1}|| ||\tau - J(q)^\top f||^2`. + + .. seealso:: `tasks/velocity/cartesian.py` + """ + + def __init__(self, model, distal_link, base_link=None, local_position=(0, 0, 0), x_desired=None, + dx_desired=None, kp=1., kd=1., weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + distal_link (int, str): distal link id or name. + base_link (int, str, None): base link id or name. If None, it will be the world. + local_position (np.array[3]): local position on the distal link. + x_desired (np.array[7], None): desired cartesian pose of distal link wrt the base. + dx_desired (np.array[6], None): desired cartesian velocity of distal link wrt the base. + kp (float, np.array[6,6]): stiffness gain. + kd (float, np.array[6,6]): damping gain. + weight (float, np.array[6,6]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(CartesianImpedanceControlTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # define variables + self.distal_link = self.model.get_link_id(distal_link) + self.base_link = self.model.get_link_id(base_link) if base_link is not None else base_link + self.local_position = local_position + self.kp = kp + self.kd = kd + + # define desired references + self.x_desired = x_desired + self.dx_desired = dx_desired + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def x_desired(self): + """Get the desired cartesian pose for the distal link wrt to the base.""" + return self._x_d + + @x_desired.setter + def x_desired(self, x_d): + """Get the desired cartesian pose for the distal link wrt to the base.""" + if x_d is None: + x_d = np.array([0.] * 6 + [1.]) + if not isinstance(x_d, np.ndarray): + raise TypeError("Expecting the given desired pose to be a np.array, instead got: {}".format(type(x_d))) + if len(x_d) != 7: + raise ValueError("Expecting the given desired pose array to be of length 7 (3 for the position, and 4 " + "for the orientation expressed as a quaternion [x,y,z,w]), instead got a length of: " + "{}".format(len(x_d))) + self._x_d = x_d + + @property + def dx_desired(self): + """Get the desired cartesian velocity for the distal link wrt to the base.""" + return self._dx_d + + @dx_desired.setter + def dx_desired(self, dx_d): + """Set the desired cartesian velocity for the distal link wrt to the base.""" + if dx_d is None: + dx_d = np.zeros(6) + if not isinstance(dx_d, np.ndarray): + raise TypeError("Expecting the given desired velocity to be a np.array, instead got: {}".format(type(dx_d))) + if len(dx_d) != 7: + raise ValueError("Expecting the given desired velocity array to be of length 6 (3 for the linear and 3 " + "for the angular part), instead got a length of: {}".format(len(dx_d))) + self._dx_d = dx_d + + @property + def kp(self): + """Return the stiffness gain.""" + return self._kp + + @kp.setter + def kp(self, kp): + """Set the stiffness gain.""" + if not isinstance(kp, (float, int, np.ndarray)): + raise TypeError("Expecting the given stiffness gain kp to be an int, float, np.array, instead got: " + "{}".format(type(kp))) + if isinstance(kp, np.ndarray) and kp.shape != (6, 6): + raise ValueError("Expecting the given stiffness gain matrix kp to be of shape {}, but instead got " + "shape: {}".format((self.x_size, self.x_size), kp.shape)) + self._kp = kp + + @property + def kd(self): + """Return the damping gain.""" + return self._kd + + @kd.setter + def kd(self, kd): + """Set the damping gain.""" + if not isinstance(kd, (float, int, np.ndarray)): + raise TypeError("Expecting the given damping gain kd to be an int, float, np.array, instead got: " + "{}".format(type(kd))) + if isinstance(kd, np.ndarray) and kd.shape != (6, 6): + raise ValueError("Expecting the given damping gain matrix kd to be of shape {}, but instead got " + "shape: {}".format((6, 6), kd.shape)) + self._kd = kd + + ########### + # Methods # + ########### + + def set_desired_references(self, x_des, dx_des=None, *args, **kwargs): + """Set the desired references. + + Args: + x_des (np.array[7], None): desired cartesian pose (position and quaternion [x,y,z,w]) of distal link wrt + the base. + dx_des (np.array[6], None): desired cartesian velocity of distal link wrt the base. + """ + self.x_desired = x_des + self.dx_desired = dx_des + + def get_desired_references(self): + """Return the desired references. + + Returns: + np.array[7]: desired cartesian pose (position and quaternion [x,y,z,w]) of distal link wrt the base. + np.array[6]: desired cartesian velocity of distal link wrt the base. + """ + return self.x_desired, self.dx_desired + + def _update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + # get useful variables + x = self.model.get_link_pose_wrt(self.distal_link, self.base_link) + dx = self.model.get_link_velocity(self.distal_link, self.base_link) + J = self.model.get_jacobian(self.distal_link, self.base_link, self.local_position) # shape: (6,N) + H = self.model.get_inertia_matrix() # shape: (N,N) + + # compute A matrix + self._A = J.dot(np.linalg.inv(H)) # shape: (6,N) + + # compute position/orientation error + position_error = (self._x_d[:3] - x[:3]) + orientation_error = quaternion_error(quat_des=self._x_d[3:], quat_cur=x[3:]) + error = np.concatenate((position_error, orientation_error)) + + # compute wrench + f = np.dot(self.kp, error) + np.dot(self.kd, (self._dx_d - dx)) # shape: (6,) + + # compute b vector + self._b = self._A.dot(J.T).dot(f) diff --git a/pyrobolearn/priorities/tasks/torque/joint_impedance_control.py b/pyrobolearn/priorities/tasks/torque/joint_impedance_control.py new file mode 100644 index 0000000..cacc23b --- /dev/null +++ b/pyrobolearn/priorities/tasks/torque/joint_impedance_control.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python +r"""Provide the joint impedance control task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import JointTorqueTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Alessio Rocchi (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class JointImpedanceControlTask(JointTorqueTask): + r"""Joint Impedance Control Task + + The joint impedance control task minimizes the specified torques given as a PD control from the desired joint + positions and velocities. That it, it minimizes: + + .. math:: || \tau - (K_p (q_d - q) + K_d (\dot{q}_d - \dot{q})) ||^2 + + where :math:`\tau` are the torques being optimized, :math:`K_p` and :math:`K_d` are the stiffness and damping + gains respectively, :math:`q` and :math:`\dot{q}` are the joint positions and velocities, and the subscript + :math:`d` means 'desired'. + + The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting + :math:`A = I` (where :math:`I` is the identity matrix), :math:`x = \tau`, and + :math:`b = K_p (q_d - q) + K_d (\dot{q}_d - \dot{q})`. + + From [1], "if used in the null-space, it realizes the null-space stiffness as described in [1]". + + .. seealso:: `tasks/velocity/postural.py` + + References: + - [1] OpenSoT framework + - [2] "Cartesian Impedance Control of Redundant and Flexible-Joint Robots", Ott, 2008 + """ + + def __init__(self, model, q_desired=None, dq_desired=None, kp=1., kd=1., weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + q_desired (np.array[N], None): desired joint positions, where :math:`N` is the number of DoFs. If None, + it will be set to 0. + dq_desired (np.array[N], None): desired joint velocities, where :math:`N` is the number of DoFs. If None, + it will be set to 0. + kp (float, np.array[N,N]): stiffness gain. + kd (float, np.array[N,N]): damping gain. + weight (float, np.array[N,N]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(JointImpedanceControlTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # define variables + self.kp = kp + self.kd = kd + + # define desired references + self.x_desired = q_desired + self.dx_desired = dq_desired + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def x_desired(self): + """Get the desired joint positions.""" + return self._q_d + + @x_desired.setter + def x_desired(self, q_d): + """Set the desired joint positions.""" + if q_d is None: + q_d = np.zeros(self.x_size) + if not isinstance(q_d, np.ndarray): + raise TypeError("Expecting the given desired joint positions to be an instance of np.array, instead got: " + "{}".format(type(q_d))) + if len(q_d) != self.x_size: + raise ValueError("Expecting the length of the given desired joint positions (={}) to be the same as the " + "number of DoFs (={})".format(len(q_d), self.x_size)) + self._q_d = q_d + + @property + def dx_desired(self): + """Get the desired joint velocities.""" + return self._dq_d + + @dx_desired.setter + def dx_desired(self, dq_d): + """Set the desired joint velocities.""" + if dq_d is None: + dq_d = np.zeros(self.x_size) + if not isinstance(dq_d, np.ndarray): + raise TypeError("Expecting the given desired joint velocities to be an instance of np.array, instead got: " + "{}".format(type(dq_d))) + if len(dq_d) != self.x_size: + raise ValueError("Expecting the length of the given desired joint velocities (={}) to be the same as the " + "number of DoFs (={})".format(len(dq_d), self.x_size)) + self._dq_d = dq_d + + @property + def kp(self): + """Return the stiffness gain.""" + return self._kp + + @kp.setter + def kp(self, kp): + """Set the stiffness gain.""" + if not isinstance(kp, (float, int, np.ndarray)): + raise TypeError("Expecting the given stiffness gain kp to be an int, float, np.array, instead got: " + "{}".format(type(kp))) + if isinstance(kp, np.ndarray) and kp.shape != (self.x_size, self.x_size): + raise ValueError("Expecting the given stiffness gain matrix kp to be of shape {}, but instead got " + "shape: {}".format((self.x_size, self.x_size), kp.shape)) + self._kp = kp + + @property + def kd(self): + """Return the damping gain.""" + return self._kd + + @kd.setter + def kd(self, kd): + """Set the damping gain.""" + if not isinstance(kd, (float, int, np.ndarray)): + raise TypeError("Expecting the given damping gain kd to be an int, float, np.array, instead got: " + "{}".format(type(kd))) + if isinstance(kd, np.ndarray) and kd.shape != (self.x_size, self.x_size): + raise ValueError("Expecting the given damping gain matrix kd to be of shape {}, but instead got " + "shape: {}".format((self.x_size, self.x_size), kd.shape)) + self._kd = kd + + ########### + # Methods # + ########### + + def set_desired_references(self, x_des, dx_des=None, *args, **kwargs): + """Set the desired references. + + Args: + x_des (np.array[N], None): desired joint positions, where :math:`N` is the number of DoFs. If None, + it will be set to 0. + dx_des (np.array[N], None): desired joint velocities, where :math:`N` is the number of DoFs. If None, + it will be set to 0. + """ + self.x_desired = x_des + self.dx_desired = dx_des + + def get_desired_references(self): + """Return the desired references. + + Returns: + np.array[N]: desired joint positions. + np.array[N]: desired joint velocities. + """ + return self.x_desired, self.dx_desired + + def _update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + q = self.model.get_joint_positions() + dq = self.model.get_joint_velocities() + + # update b vector + self._b = np.dot(self.kp, self._q_d - q) + np.dot(self.kd, self._dq_d - dq) # shape: (N,) diff --git a/pyrobolearn/priorities/tasks/velocity/__init__.py b/pyrobolearn/priorities/tasks/velocity/__init__.py new file mode 100644 index 0000000..011519d --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/__init__.py @@ -0,0 +1,32 @@ + +from .angular_momentum import AngularMomentumTask + +from .cartesian import CartesianTask + +from .com import CoMTask + +from .contact import ContactTask + +from .gaze import GazeTask + +from .interaction import InteractionTask + +from .linear_momentum import LinearMomentumTask + +from .manipulability import ManipulabilityTask + +from .minimum_acceleration import MinAccelerationTask + +from .minimum_effort import MinEffortTask + +from .minimum_velocity import MinVelocityTask + +from .momentum import CentroidalMomentumTask + +from .postural import PosturalTask + +from .pure_rolling import PureRollingTask + +from .rigid_rotation import RigidRotationTask + +from .unicycle import UnicycleTask diff --git a/pyrobolearn/priorities/tasks/velocity/angular_momentum.py b/pyrobolearn/priorities/tasks/velocity/angular_momentum.py new file mode 100644 index 0000000..a58d243 --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/angular_momentum.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python +r"""Provide the angular momentum task. + + +The implementation of this class is inspired by [1, 2] (where [1] is licensed under the LGPLv2) + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + - [2] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Xin, 2018 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import JointVelocityTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class AngularMomentumTask(JointVelocityTask): + r"""CoM Angular Momentum Task + + The is the angular part of the `CentroidalMomentumTask`. The centroidal momentum task tries to minimize the + difference between the desired and current centroidal linear moment given by: + + .. math:: ||A_G \dot{q} - h_{G,d}||^2 + + where :math:`A_G \in \mathbb{R}^{6 \times N}` is the centroidal momentum matrix (CMM, see below for description), + :math:`\dot{q}` are the joint velocities being optimized, and :math:`h_{G,d}` is the desired centroidal momentum. + + This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=A_G`, :math:`x=\dot{q}`, + and :math:`b = h_{G,d}`. + + The centroidal momentum (which is the sum of all body spatial momenta computed wrt the CoM) is given by: + + .. math:: h_G = A_G \dot{q} + + where :math:`h_G = [k_G^\top, l_G^\top]^\top \in \mathbb{R}^6` is the centroidal momentum (the subscript :math:`G` + denotes the CoM) with :math:`k_G \in \mathbb{R}^3` being the angular momentum and :math:`l_G \in \mathbb{R}^3` the + linear part, :math:`\dot{q}` are the joint velocities, and :math:`A_G \in \mathbb{R}^{6 \times N}` (with :math:`N` + is the number of DoFs) is the centroidal momentum matrix (CMM). + + "The CMM is computed from the joint space inertia matrix :math:`H(q)`, given by: + + .. math:: A_G = ^1X_G^\top S_1 H(q) = ^1X_G^\top H_1(q) + + where :math:`^1X_G^\top \in \mathbb{R}^{6 \times 6}` is the spatial transformation matrix that transfers spatial + momentum from the floating base (Body 1) to the CoM (G), :math:`H(q)` is the full joint space inertia matrix, + :math:`H_1 = S_1 H` is the floating base (Body 1) inertia matrix selected using the selector matrix + :math:`S_1 = [1_{6 \times 6}, 0_{6 \times (N-6)}}`. + + The spatial transformation matrix is given by: + + .. math:: + + ^1X_G^\top = \left[ \begin{array}{cc} + ^GR_1 & ^GR_1 S(^1p_G)^\top \\ + 0 & ^GR_1 + \\end{array} \right] + + where :math:`^GR_1` is the rotation matrix of :math:`G` wrt the floating base (Body 1), + :math:`^1p_G = ^1R_0 (^0p_G - ^0p_1)` is the position vector from the floating base (Body 1) origin to the CoM + expressed in the floating base (Body 1) frame, :math:`S(\cdot)` provides the skew symmetric cross product matrix + such that :math:`S(p)v = p \cross v`. Note that the orientation of Frame :math:`G` (CoM) could be selected to be + parallel to the ground inertial (i.e. world) frame then the rotation matrix :math:`^GR_1 = ^0R_1`." [3] + + + The centroidal angular momentum task focuses on the angular part :math:`k_G \in \mathbb{R}^3` in the centroidal + momentum :math:`h_G = [k_G^\top, l_G^\top]^\top \in \mathbb{R}^6`. + + + References: + - [1] "Centroidal momentum matrix of a humanoid robot: structure and properties", Orin et al., 2008 + - [2] "Centroidal dynamics of a humanoid robot", Orin et al., 2013 + - [3] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Xin, 2018 + """ + + def __init__(self, model, k_desired=None, weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + k_desired (np.array[3], None): desired centroidal angular momentum. + weight (float, np.array[3,3]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(AngularMomentumTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # define desired reference + self.x_desired = k_desired + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def x_desired(self): + """Get the desired centroidal angular momentum.""" + return self._k_desired + + @x_desired.setter + def x_desired(self, k_d): + """Set the desired centroidal angular momentum.""" + if k_d is None: + k_d = np.zeros(3) + if not isinstance(k_d, np.ndarray): + raise TypeError("Expecting the given desired centroidal angular momentum to be an instance of np.array, " + "instead got: {}".format(type(k_d))) + if len(k_d) != 3: + raise ValueError("Expecting the length of the given desired angular centroidal momentum to be of length " + "3, instead got: {}".format(len(k_d))) + self._k_desired = k_d + + ########### + # Methods # + ########### + + def set_desired_references(self, x_des, *args, **kwargs): + """Set the desired references. + + Args: + x_des (np.array[3], None): desired centroidal angular momentum. + """ + self.x_desired = x_des + + def get_desired_references(self): + """Return the desired references. + + Returns: + np.array[3]: desired centroidal angular momentum. + """ + return self.x_desired + + def _update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + self._A = self.model.get_centroidal_momentum_matrix()[:3] # shape: (3, N) + self._b = self._k_desired # shape: (3,) diff --git a/pyrobolearn/priorities/tasks/velocity/cartesian.py b/pyrobolearn/priorities/tasks/velocity/cartesian.py new file mode 100644 index 0000000..9240a80 --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/cartesian.py @@ -0,0 +1,170 @@ +#!/usr/bin/env python +r"""Provide the cartesian (velocity) task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import JointVelocityTask +from pyrobolearn.utils.transformation import quaternion_error + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Alessio Rocchi (C++)", "Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class CartesianTask(JointVelocityTask): + r"""Cartesian (velocity) Task + + The cartesian task tries to impose a desired pose (position and orientation) of a distal link with respect to a + base link or the world frame. The minimization problem is given by: + + .. math:: || ^bJ_d(q) \dot{q} - (K_p e + \dot{x}_d) ||^2 + + where :math:`^bJ_d(q) \in \mathbb{R}^{6 \times N}` is the Jacobian taken from the base to the distal link, + :math:`\dot{q}` are the joint velocities being optimized, :math:`K_p` is the stiffness gain, + :math:`e \in \mathbb{R}^{6}` is the error which is the concatenation of the position error given by + :math:`e_{p} = (x_d - x)` (with :math:`x_d` being the desired pose, and :math:`x` the current pose), and the + orientation error given by (if expressed as quaternions :math:`o = {s, v}` where :math:`s` is the real scalar part, + and :math:`v` is the vector part) :math:`e_{o} = s v_d - s_d v - v_d \cross v`, and :math:`\dot{x}_d` is the + desired cartesian velocity for the distal link with respect to the base link. + + This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = ^bJ_d(q)`, + :math:`x = \dot{q}`, and :math:`b = K_p e + \dot{x}_d`. + """ + + def __init__(self, model, distal_link, base_link=None, local_position=(0, 0, 0), x_desired=None, + dx_desired=None, kp=1., weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + distal_link (int, str): distal link id or name. + base_link (int, str, None): base link id or name. If None, it will be the world. + local_position (np.array[3]): local position on the distal link. + x_desired (np.array[7], None): desired cartesian pose of distal link wrt the base. + dx_desired (np.array[6], None): desired cartesian velocity of distal link wrt the base. + kp (float, np.array[6,6]): stiffness gain. + weight (float, np.array[6,6]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(CartesianTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # define variables + self.distal_link = self.model.get_link_id(distal_link) + self.base_link = self.model.get_link_id(base_link) if base_link is not None else base_link + self.local_position = local_position + self.kp = kp + + # define desired references + self.x_desired = x_desired + self.dx_desired = dx_desired + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def x_desired(self): + """Get the desired cartesian pose for the distal link wrt to the base.""" + return self._x_d + + @x_desired.setter + def x_desired(self, x_d): + """Get the desired cartesian pose for the distal link wrt to the base.""" + if x_d is None: + x_d = np.array([0.]*6 + [1.]) + if not isinstance(x_d, np.ndarray): + raise TypeError("Expecting the given desired pose to be a np.array, instead got: {}".format(type(x_d))) + if len(x_d) != 7: + raise ValueError("Expecting the given desired pose array to be of length 7 (3 for the position, and 4 " + "for the orientation expressed as a quaternion [x,y,z,w]), instead got a length of: " + "{}".format(len(x_d))) + self._x_d = x_d + + @property + def dx_desired(self): + """Get the desired cartesian velocity for the distal link wrt to the base.""" + return self._dx_d + + @dx_desired.setter + def dx_desired(self, dx_d): + """Set the desired cartesian velocity for the distal link wrt to the base.""" + if dx_d is None: + dx_d = np.zeros(6) + if not isinstance(dx_d, np.ndarray): + raise TypeError("Expecting the given desired velocity to be a np.array, instead got: {}".format(type(dx_d))) + if len(dx_d) != 7: + raise ValueError("Expecting the given desired velocity array to be of length 6 (3 for the linear and 3 " + "for the angular part), instead got a length of: {}".format(len(dx_d))) + self._dx_d = dx_d + + @property + def kp(self): + """Return the stiffness gain.""" + return self._kp + + @kp.setter + def kp(self, kp): + """Set the stiffness gain.""" + if not isinstance(kp, (float, int, np.ndarray)): + raise TypeError("Expecting the given stiffness gain kp to be an int, float, np.array, instead got: " + "{}".format(type(kp))) + if isinstance(kp, np.ndarray) and kp.shape != (6, 6): + raise ValueError("Expecting the given stiffness gain matrix kp to be of shape {}, but instead got " + "shape: {}".format((6, 6), kp.shape)) + self._kp = kp + + ########### + # Methods # + ########### + + def set_desired_references(self, x_des, dx_des=None, *args, **kwargs): + """Set the desired references. + + Args: + x_des (np.array[7], None): desired cartesian pose (position and quaternion [x,y,z,w]) of distal link wrt + the base. + dx_des (np.array[6], None): desired cartesian velocity of distal link wrt the base. + """ + self.x_desired = x_des + self.dx_desired = dx_des + + def get_desired_references(self): + """Return the desired references. + + Returns: + np.array[7]: desired cartesian pose (position and quaternion [x,y,z,w]) of distal link wrt the base. + np.array[6]: desired cartesian velocity of distal link wrt the base. + """ + return self.x_desired, self.dx_desired + + def _update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + x = self.model.get_link_pose_wrt(self.distal_link, self.base_link) + self._A = self.model.get_jacobian(self.distal_link, self.base_link, self.local_position) # shape: (6,N) + + # compute position/orientation error + position_error = (self._x_d[:3] - x[:3]) + orientation_error = quaternion_error(quat_des=self._x_d[3:], quat_cur=x[3:]) + error = np.concatenate((position_error, orientation_error)) + + # compute b vector + self._b = np.dot(self.kp, error) + self._dx_d # shape: (6,) diff --git a/pyrobolearn/priorities/tasks/velocity/com.py b/pyrobolearn/priorities/tasks/velocity/com.py new file mode 100644 index 0000000..12e9186 --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/com.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python +r"""Provide the center of mass velocity task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import JointVelocityTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Alessio Rocchi (C++)", "Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class CoMTask(JointVelocityTask): + r"""Center of Mass Velocity Task + + The CoM task tries to impose a desired position of the CoM with respect to the world frame. + + .. math:: ||J_{CoM} \dot{q} - (K_p (x_d - x) + \dot{x}_d)||^2 + + where :math:`J_{CoM}` is the CoM Jacobian, :math:`\dot{q}` are the joint velocities being optimized, :math:`K_p` + is the stiffness gain, :math:`x_d` and :math:`x` are the desired and current cartesian CoM position + respectively, and :math:`\dot{x}_d` is the desired linear velocity of the CoM. + + This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=J_{CoM}`, + :math:`x=\dot{q}`, and :math:`b = K_p (x_d - x) + \dot{x}_d`. + """ + + def __init__(self, model, x_desired=None, dx_desired=None, kp=1., weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + x_desired (np.array[3], None): desired CoM position. If None, it will be set to 0. + dx_desired (np.array[3], None): desired CoM linear velocity. If None, it will be set to 0. + kp (float, np.array[3,3]): stiffness gain. + weight (float, np.array[3,3]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(CoMTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # define variable + self.kp = kp + + # define desired references + self.x_desired = x_desired + self.dx_desired = dx_desired + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def x_desired(self): + """Get the desired CoM position.""" + return self._x_d + + @x_desired.setter + def x_desired(self, x_d): + """Set the desired CoM position.""" + if x_d is None: + x_d = np.zeros(3) + if not isinstance(x_d, np.ndarray): + raise TypeError("Expecting the given desired CoM position to be a np.array, instead got: " + "{}".format(type(x_d))) + if len(x_d) != 3: + raise ValueError("Expecting the given desired CoM position array to be of length 3, instead got a length " + "of: {}".format(len(x_d))) + self._x_d = x_d + + @property + def dx_desired(self): + """Get the desired CoM linear velocity.""" + return self._dx_d + + @dx_desired.setter + def dx_desired(self, dx_d): + """Set the desired CoM linear velocity.""" + if dx_d is None: + dx_d = np.zeros(3) + if not isinstance(dx_d, np.ndarray): + raise TypeError("Expecting the given desired CoM linear velocity to be a np.array, instead got: " + "{}".format(type(dx_d))) + if len(dx_d) != 3: + raise ValueError("Expecting the given desired CoM linear velocity array to be of length 3, instead got a " + "length of: {}".format(len(dx_d))) + self._dx_d = dx_d + + @property + def kp(self): + """Return the stiffness gain.""" + return self._kp + + @kp.setter + def kp(self, kp): + """Set the stiffness gain.""" + if not isinstance(kp, (float, int, np.ndarray)): + raise TypeError("Expecting the given stiffness gain kp to be an int, float, np.array, instead got: " + "{}".format(type(kp))) + if isinstance(kp, np.ndarray) and kp.shape != (3, 3): + raise ValueError("Expecting the given stiffness gain matrix kp to be of shape {}, but instead got " + "shape: {}".format((self.x_size, self.x_size), kp.shape)) + self._kp = kp + + ########### + # Methods # + ########### + + def set_desired_references(self, x_des, dx_des=None, *args, **kwargs): + """Set the desired references. + + Args: + x_des (np.array[3], None): desired CoM position. If None, it will be set to 0. + dx_des (np.array[3], None): desired CoM linear velocity. If None, it will be set to 0. + """ + self.x_desired = x_des + self.dx_desired = dx_des + + def get_desired_references(self): + """Return the desired references. + + Returns: + np.array[3]: desired CoM position. + np.array[3]: desired CoM linear velocity. + """ + return self.x_desired, self.dx_desired + + def _update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + x = self.model.get_com_position() + self._A = self.model.get_com_jacobian() # shape: (3, N) + self._b = np.dot(self.kp, (self._x_d - x)) + self._dx_d # shape: (3,) diff --git a/pyrobolearn/priorities/tasks/velocity/contact.py b/pyrobolearn/priorities/tasks/velocity/contact.py new file mode 100644 index 0000000..43cce6f --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/contact.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python +r"""Provide the contact task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import JointVelocityTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Alessio Rocchi (C++)", "Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class ContactTask(JointVelocityTask): + r"""Contact Task + + The contact task tries to minimize the movement of a contact link: + + .. math:: || C J_c(q) \dot{q} ||^2, + + where :math:`C \in \mathbb{R}^{6 \times 6}` is the contact matrix (=a diagonal selector matrix where the entries + are 1 for cartesian velocities that we wish to minimize such that the link doesn't move, and 0 for cartesian + velocities that are free to change), :math:`J_c(q)` is the contact Jacobian (the Jacobian from the world frame to + the contact point), and :math:`\dot{q}` are the joint velocities being optimized. + + This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = C J_c(q)`, + :math:`x = \dot{q}`, and :math:`b = 0`. + """ + + def __init__(self, model, distal_link, contact_matrix=1., weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + distal_link (int, str): distal link id or name. + contact_matrix (np.array[6,6], None): contact selector matrix (=a diagonal square matrix). If None, by + default it will be set to the identity matrix. + weight (float, np.array[6,6]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(ContactTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # set variable + self.distal_link = self.model.get_link_id(distal_link) + self.contact_matrix = contact_matrix + + # set QP vector + self._b = np.zeros(6) + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def contact_matrix(self): + """Return the contact selector matrix.""" + return self._contact_matrix + + @contact_matrix.setter + def contact_matrix(self, matrix): + """Set the contact selector matrix.""" + if matrix is None: + matrix = np.identity(6) + + # check contact matrix type + if not isinstance(matrix, (int, float, np.ndarray)): + raise TypeError("Expecting the given contact matrix to be an int, float, or diagonal np.array, instead " + "got: {}".format(type(matrix))) + + # if numpy array, check its shape and make sure it is a diagonal matrix + if isinstance(matrix, np.ndarray): + if matrix.shape != (6, 6): + raise ValueError("Expecting the given contact matrix to be of shape (6,6), instead got a shape of: " + "{}".format(type(matrix))) + + # make sure the contact matrix is a diagonal matrix + matrix = np.diag(np.diag(matrix)) + + # set the contact matrix + self._contact_matrix = matrix + + ########### + # Methods # + ########### + + def _update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + self._A = np.dot(self.contact_matrix, self.model.get_jacobian(self.distal_link)) diff --git a/pyrobolearn/priorities/tasks/velocity/gaze.py b/pyrobolearn/priorities/tasks/velocity/gaze.py new file mode 100644 index 0000000..7e8a1f8 --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/gaze.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python +r"""Provide the gaze task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +# TODO: finish to implement this class + +import numpy as np + +from pyrobolearn.priorities.tasks import JointVelocityTask +from pyrobolearn.priorities.tasks.velocity import CartesianTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Alessio Rocchi (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class GazeTask(JointVelocityTask): + r"""Gaze Task + + The Gaze class implement a Cartesian Task in which the gaze of the robot is controlled. This is achieved by + controlling the orientation of the distal link equipped with a camera (this can for instance be the head of a + robot) with respect to a base frame (which can be the neck or waist for instance). For this purpose, from the + Cartesian sub-task only the pitch and yaw velocities are considered. + + The implementation is based on [1] which itself is inspired from [2]. + + References: + - [1] OpenSoT framework + - [2] "Adaptive Predictive Gaze Control of a Redundant Humanoid Robot Head", Milighetti et al., 2011 + """ + + def __init__(self, model, weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + weight (float, np.array[2,2]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(GazeTask, self).__init__(model=model, constraints=constraints) + + # self.cartesian_task = CartesianTask(self.model, distal_link=distal_link, weight=weight) + + raise NotImplementedError("This class has not been implemented yet.") diff --git a/pyrobolearn/priorities/tasks/velocity/interaction.py b/pyrobolearn/priorities/tasks/velocity/interaction.py new file mode 100644 index 0000000..f6d6277 --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/interaction.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python +r"""Provide the interaction task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +# TODO: finish to implement this class + +import numpy as np + +from pyrobolearn.priorities.tasks import JointVelocityTask +from pyrobolearn.priorities.tasks.velocity import CartesianTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["OpenSoT (Enrico Mingo Hoffman, Alessio Rocchi) (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class InteractionTask(JointVelocityTask): + r"""Interaction Task + + From [1], "The Interaction class implements an Admittance based force control using the admittance law: + + .. math:: + + dx = K_p * (w_d - w) \\ + x_d = x + dx + + where :math:`w_d \in \mathbb{R}^6` is the desired wrench in some base_link frame, :math:`w` is the measured wrench + transformed from the Force/Torque sensor frame to the base_link frame. The displacement :math:`dx` is integrated + using the previous position :math:`x`, and a new desired position :math:`x_d` is computed. The references + :math:`x_d` and :math:`dx` are then used inside a Cartesian task (see ``CartesianTask``). + + Warnings: the :math:`w_d` is the desired wrench that the robot has to exert on the environment, so the measured + wrench :math:`w` is the wrench produced by the robot on the environment (and not the opposite)!" + + References: + - [1] OpenSoT framework + """ + + def __init__(self, model, distal_link, base_link=-1, desired_wrench=0., weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + distal_link (int, str): distal link id or name. + base_link (int, str, None): base link id or name. If None, it will be the base root link. + desired_wrench (float, np.array[6]): desired wrench. + weight (float, np.array[6,6]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(InteractionTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # create sub-task + self._task = CartesianTask(model, distal_link=distal_link, base_link=base_link, weight=weight) + + raise NotImplementedError("This class has not been implemented yet.") + + def update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + self._A = self._task.A + self._b = self._task.b diff --git a/pyrobolearn/priorities/tasks/velocity/linear_momentum.py b/pyrobolearn/priorities/tasks/velocity/linear_momentum.py new file mode 100644 index 0000000..d927fd6 --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/linear_momentum.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python +r"""Provide the linear momentum task. + + +The implementation of this class is inspired by [1, 2] (where [1] is licensed under the LGPLv2) + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + - [2] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Xin, 2018 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import JointVelocityTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Pouya Mohammadi (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class LinearMomentumTask(JointVelocityTask): + r"""(Centroidal) Linear Momentum Task + + The is the linear part of the `CentroidalMomentumTask`. The centroidal momentum task tries to minimize the + difference between the desired and current centroidal linear moment given by: + + .. math:: ||A_G \dot{q} - h_{G,d}||^2 + + where :math:`A_G \in \mathbb{R}^{6 \times N}` is the centroidal momentum matrix (CMM, see below for description), + :math:`\dot{q}` are the joint velocities being optimized, and :math:`h_{G,d}` is the desired centroidal momentum. + + This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=A_G`, :math:`x=\dot{q}`, + and :math:`b = h_{G,d}`. + + The centroidal momentum (which is the sum of all body spatial momenta computed wrt the CoM) is given by: + + .. math:: h_G = A_G \dot{q} + + where :math:`h_G = [k_G^\top, l_G^\top]^\top \in \mathbb{R}^6` is the centroidal momentum (the subscript :math:`G` + denotes the CoM) with :math:`k_G \in \mathbb{R}^3` being the angular momentum and :math:`l_G \in \mathbb{R}^3` the + linear part, :math:`\dot{q}` are the joint velocities, and :math:`A_G \in \mathbb{R}^{6 \times N}` (with :math:`N` + is the number of DoFs) is the centroidal momentum matrix (CMM). + + "The CMM is computed from the joint space inertia matrix :math:`H(q)`, given by: + + .. math:: A_G = ^1X_G^\top S_1 H(q) = ^1X_G^\top H_1(q) + + where :math:`^1X_G^\top \in \mathbb{R}^{6 \times 6}` is the spatial transformation matrix that transfers spatial + momentum from the floating base (Body 1) to the CoM (G), :math:`H(q)` is the full joint space inertia matrix, + :math:`H_1 = S_1 H` is the floating base (Body 1) inertia matrix selected using the selector matrix + :math:`S_1 = [1_{6 \times 6}, 0_{6 \times (N-6)}}`. + + The spatial transformation matrix is given by: + + .. math:: + + ^1X_G^\top = \left[ \begin{array}{cc} + ^GR_1 & ^GR_1 S(^1p_G)^\top \\ + 0 & ^GR_1 + \\end{array} \right] + + where :math:`^GR_1` is the rotation matrix of :math:`G` wrt the floating base (Body 1), + :math:`^1p_G = ^1R_0 (^0p_G - ^0p_1)` is the position vector from the floating base (Body 1) origin to the CoM + expressed in the floating base (Body 1) frame, :math:`S(\cdot)` provides the skew symmetric cross product matrix + such that :math:`S(p)v = p \cross v`. Note that the orientation of Frame :math:`G` (CoM) could be selected to be + parallel to the ground inertial (i.e. world) frame then the rotation matrix :math:`^GR_1 = ^0R_1`." [3] + + + The centroidal linear momentum task focuses on the linear part :math:`l_G \in \mathbb{R}^3` in the centroidal + momentum :math:`h_G = [k_G^\top, l_G^\top]^\top \in \mathbb{R}^6`. + + + References: + - [1] "Centroidal momentum matrix of a humanoid robot: structure and properties", Orin et al., 2008 + - [2] "Centroidal dynamics of a humanoid robot", Orin et al., 2013 + - [3] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Xin, 2018 + """ + + def __init__(self, model, l_desired=None, weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + l_desired (np.array[3], None): desired centroidal linear momentum. + weight (float, np.array[3,3]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(LinearMomentumTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # define desired reference + self.x_desired = l_desired + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def x_desired(self): + """Get the desired centroidal linear momentum.""" + return self._l_desired + + @x_desired.setter + def x_desired(self, l_d): + """Set the desired centroidal linear momentum.""" + if l_d is None: + l_d = np.zeros(3) + if not isinstance(l_d, np.ndarray): + raise TypeError("Expecting the given desired centroidal linear momentum to be an instance of np.array, " + "instead got: {}".format(type(l_d))) + if len(l_d) != 3: + raise ValueError("Expecting the length of the given desired linear centroidal momentum to be of length 3, " + "instead got: {}".format(len(l_d))) + self._l_desired = l_d + + ########### + # Methods # + ########### + + def set_desired_references(self, x_des, *args, **kwargs): + """Set the desired references. + + Args: + x_des (np.array[3], None): desired centroidal linear momentum. + """ + self.x_desired = x_des + + def get_desired_references(self): + """Return the desired references. + + Returns: + np.array[3]: desired centroidal linear momentum. + """ + return self.x_desired + + def _update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + self._A = self.model.get_centroidal_momentum_matrix()[3:] # shape: (3, N) + self._b = self._l_desired # shape: (3,) diff --git a/pyrobolearn/priorities/tasks/velocity/manipulability.py b/pyrobolearn/priorities/tasks/velocity/manipulability.py new file mode 100644 index 0000000..bab1564 --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/manipulability.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python +r"""Provide the manipulability task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +# TODO: finish to implement this + +import numpy as np + +from pyrobolearn.priorities.tasks import JointVelocityTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class ManipulabilityTask(JointVelocityTask): + r"""Manipulability Task + + The manipulability task implements a tasks that tries to maximize the manipulability measure given in [1]: + + .. math:: w(q) = \sqrt{ \det( J(q) W J(q)^\top ) } + + where :math:`W` is a constant weight matrix, :math:`q` are the joint positions, and :math:`J(q)` is the jacobian. + The gradient of :math:`w` is then computed and projected using the gradient projection method [2]. + + The quadratic cost being minimized is: + + .. math:: ||\dot{q} - \dot{q}_0||^2 + + where :math:`\dot{q}` are the joint velocities being optimized, + :math:`\dot{q}_0 = k_0 \left( \frac{\partial w(q)}{\partial q} \right)^\top` where :math:`k_0 > 0` and + :math:`w(q)` is an objective function of the joint variables, where in this case, the manipulability measure is + given by :math:`w(q) = \sqrt{\det( J(q) J^\top(q) )}`. By maximizing this measure, we move away from singularities. + + References: + - [1] "Robotics: Modelling, Planning, and Control", Siciliano et al., 2010 + - [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + """ + + def __init__(self, model, constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface + constraints (list of Constraint): list of constraints associated with the task. + """ + super(ManipulabilityTask, self).__init__(model=model, constraints=constraints) + + raise NotImplementedError("This class has not been implemented yet.") diff --git a/pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py b/pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py new file mode 100644 index 0000000..013c9fb --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python +r"""Provide the minimum acceleration task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import JointVelocityTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class MinAccelerationTask(JointVelocityTask): + r"""Minimum Acceleration Task + + The minimum acceleration task tries to minimize the change in velocity, that is, it minimizes: + + .. math:: || \dot{q}_t - \dot{q}_{t-1} ||^2 + + where :math:`\dot{q}_t` are the current joint velocities being optimized, and :math:`\dot{q}_{t-1}` are the + previous joint velocities. + + This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = I`, :math:`x = \dot{q}`, + and :math:`b = \dot{q}_{t-1}`. + """ + + def __init__(self, model, weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + weight (float, np.array[N,N]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(MinAccelerationTask, self).__init__(model=model, weight=weight, constraints=constraints) + + ########### + # Methods # + ########### + + def update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + self._b = self.model.get_joint_velocities() diff --git a/pyrobolearn/priorities/tasks/velocity/minimum_effort.py b/pyrobolearn/priorities/tasks/velocity/minimum_effort.py new file mode 100644 index 0000000..83d290a --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/minimum_effort.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python +r"""Provide the minimum effort task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +# TODO: finish to implement this + +import numpy as np + +from pyrobolearn.priorities.tasks import JointVelocityTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Alessio Rocchi (C++)", "Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class MinEffortTask(JointVelocityTask): + r"""Minimum Effort Task + + "This class implements a task that tries to bring the robot in a minimum-effort posture." [1] + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + """ + + def __init__(self, model, weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + weight (float, np.array[N,N]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(MinEffortTask, self).__init__(model=model, weight=weight, constraints=constraints) + + raise NotImplementedError("This class has not been implemented yet.") diff --git a/pyrobolearn/priorities/tasks/velocity/minimum_velocity.py b/pyrobolearn/priorities/tasks/velocity/minimum_velocity.py new file mode 100644 index 0000000..8d569b9 --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/minimum_velocity.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python +r"""Provide the minimum velocity task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +from pyrobolearn.priorities.tasks import JointVelocityTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Alessio Rocchi (C++)", "Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class MinVelocityTask(JointVelocityTask): + r"""Minimum Velocity Task + + The minimum velocity task minimizes the joint velocities, that is it minimizes: + + .. math:: ||\dot{q}||^2, + + which is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=I`, :math:`x=\dot{q}`, + and :math:`b=0`. + + This minimum velocity task is often used in conjunction with other tasks such as `PosturalTask` or `CartesianTask`. + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + """ + + def __init__(self, model, weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + weight (float, np.array[N,N]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + # the variables A and b are initialized by default to be A=I and b=0 + super(MinVelocityTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # first update + self.update() diff --git a/pyrobolearn/priorities/tasks/velocity/momentum.py b/pyrobolearn/priorities/tasks/velocity/momentum.py new file mode 100644 index 0000000..592800e --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/momentum.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python +r"""Provide the CoM linear and angular momentum task. + + +The implementation of this class is inspired by [1, 2] (where [1] is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + - [2] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Xin, 2018 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import JointVelocityTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Songyan Xin (insight)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class CentroidalMomentumTask(JointVelocityTask): + r"""Centroidal Momentum Task + + The centroidal momentum task tries to minimize the difference between the desired and current centroidal moment + given by: + + .. math:: ||A_G \dot{q} - h_{G,d}||^2 + + where :math:`A_G \in \mathbb{R}^{6 \times N}` is the centroidal momentum matrix (CMM, see below for description), + :math:`\dot{q}` are the joint velocities being optimized, and :math:`h_{G,d}` is the desired centroidal momentum. + + This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=A_G`, :math:`x=\dot{q}`, + and :math:`b = h_{G,d}`. + + The centroidal momentum (which is the sum of all body spatial momenta computed wrt the CoM) is given by: + + .. math:: h_G = A_G \dot{q} + + where :math:`h_G = [k_G^\top, l_G^\top]^\top \in \mathbb{R}^6` is the centroidal momentum (the subscript :math:`G` + denotes the CoM) with :math:`k_G \in \mathbb{R}^3` being the angular momentum and :math:`l_G \in \mathbb{R}^3` the + linear part, :math:`\dot{q}` are the joint velocities, and :math:`A_G \in \mathbb{R}^{6 \times N}` (with :math:`N` + is the number of DoFs) is the centroidal momentum matrix (CMM). + + "The CMM is computed from the joint space inertia matrix :math:`H(q)`, given by: + + .. math:: A_G = ^1X_G^\top S_1 H(q) = ^1X_G^\top H_1(q) + + where :math:`^1X_G^\top \in \mathbb{R}^{6 \times 6}` is the spatial transformation matrix that transfers spatial + momentum from the floating base (Body 1) to the CoM (G), :math:`H(q)` is the full joint space inertia matrix, + :math:`H_1 = S_1 H` is the floating base (Body 1) inertia matrix selected using the selector matrix + :math:`S_1 = [1_{6 \times 6}, 0_{6 \times (N-6)}}`. + + The spatial transformation matrix is given by: + + .. math:: + + ^1X_G^\top = \left[ \begin{array}{cc} + ^GR_1 & ^GR_1 S(^1p_G)^\top \\ + 0 & ^GR_1 + \\end{array} \right] + + where :math:`^GR_1` is the rotation matrix of :math:`G` wrt the floating base (Body 1), + :math:`^1p_G = ^1R_0 (^0p_G - ^0p_1)` is the position vector from the floating base (Body 1) origin to the CoM + expressed in the floating base (Body 1) frame, :math:`S(\cdot)` provides the skew symmetric cross product matrix + such that :math:`S(p)v = p \cross v`. Note that the orientation of Frame :math:`G` (CoM) could be selected to be + parallel to the ground inertial (i.e. world) frame then the rotation matrix :math:`^GR_1 = ^0R_1`." [3] + + References: + - [1] "Centroidal momentum matrix of a humanoid robot: structure and properties", Orin et al., 2008 + - [2] "Centroidal dynamics of a humanoid robot", Orin et al., 2013 + - [3] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Xin, 2018 + """ + + def __init__(self, model, h_desired=None, weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + h_desired (np.array[6], None): desired centroidal momentum which is the concatenation of the desired + angular and linear momentum. If None, it will be set to zero. + weight (float, np.array[6,6]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(CentroidalMomentumTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # define desired reference + self.x_desired = h_desired + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def x_desired(self): + """Get the desired centroidal momentum (angular and linear momentum).""" + return self._h_desired + + @x_desired.setter + def x_desired(self, h_d): + """Set the desired centroidal momentum (angular and linear momentum).""" + if h_d is None: + h_d = np.zeros(6) + if not isinstance(h_d, np.ndarray): + raise TypeError("Expecting the given desired centroidal momentum to be an instance of np.array, instead " + "got: {}".format(type(h_d))) + if len(h_d) != 6: + raise ValueError("Expecting the length of the given desired centroidal momentum to be of length 6, " + "instead got: {}".format(len(h_d))) + self._h_desired = h_d + + ########### + # Methods # + ########### + + def set_desired_references(self, x_des, *args, **kwargs): + """Set the desired references. + + Args: + x_des (np.array[6], None): desired centroidal momentum (angular and linear momentum). + """ + self.x_desired = x_des + + def get_desired_references(self): + """Return the desired references. + + Returns: + np.array[6]: desired centroidal momentum (angular and linear momentum). + """ + return self.x_desired + + def _update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + self._A = self.model.get_centroidal_momentum_matrix() # shape: (6, N) + self._b = self._h_desired # shape: (6,) diff --git a/pyrobolearn/priorities/tasks/velocity/postural.py b/pyrobolearn/priorities/tasks/velocity/postural.py new file mode 100644 index 0000000..313cf5b --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/postural.py @@ -0,0 +1,156 @@ +#!/usr/bin/env python +r"""Provide the postural (velocity) task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +import numpy as np + +from pyrobolearn.priorities.tasks import JointVelocityTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Alessio Rocchi (C++)", "Enrico Mingo Hoffman (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class PosturalTask(JointVelocityTask): + r"""Postural Task + + The postural task tries to bring the robot to a reference posture; that is, it minimizes the joint velocities such + that it gets close to the specified posture (given by the desired joint positions and velocities): + + .. math:: || \dot{q} - (K_p (q_d - q) + \dot{q}_d) ||^2, + + which is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=I`, :math:`x=\dot{q}`, + and :math:`b = K_p (q_d - q) + \dot{q}_d`, where :math:`K_p` is the stiffness gain and the subscript :math:`d` + means "desired". + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + """ + + def __init__(self, model, q_desired=None, dq_desired=None, kp=1., weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + q_desired (np.array[N], None): desired joint positions, where :math:`N` is the number of DoFs. If None, + it will be set to 0. + dq_desired (np.array[N], None): desired joint velocities, where :math:`N` is the number of DoFs. If None, + it will be set to 0. + kp (float, np.array[N,N]): stiffness gain. + weight (float, np.array[N,N]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(PosturalTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # define variables + self.kp = kp + + # define desired references + self.x_desired = q_desired + self.dx_desired = dq_desired + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def x_desired(self): + """Get the desired joint positions.""" + return self._q_d + + @x_desired.setter + def x_desired(self, q_d): + """Set the desired joint positions.""" + if q_d is None: + q_d = np.zeros(self.x_size) + if not isinstance(q_d, np.ndarray): + raise TypeError("Expecting the given desired joint positions to be an instance of np.array, instead got: " + "{}".format(type(q_d))) + if len(q_d) != self.x_size: + raise ValueError("Expecting the length of the given desired joint positions (={}) to be the same as the " + "number of DoFs (={})".format(len(q_d), self.x_size)) + self._q_d = q_d + + @property + def dx_desired(self): + """Get the desired joint velocities.""" + return self._dq_d + + @dx_desired.setter + def dx_desired(self, dq_d): + """Set the desired joint velocities.""" + if dq_d is None: + dq_d = np.zeros(self.x_size) + if not isinstance(dq_d, np.ndarray): + raise TypeError("Expecting the given desired joint velocities to be an instance of np.array, instead got: " + "{}".format(type(dq_d))) + if len(dq_d) != self.x_size: + raise ValueError("Expecting the length of the given desired joint velocities (={}) to be the same as the " + "number of DoFs (={})".format(len(dq_d), self.x_size)) + self._dq_d = dq_d + + @property + def kp(self): + """Return the stiffness gain.""" + return self._kp + + @kp.setter + def kp(self, kp): + """Set the stiffness gain.""" + if not isinstance(kp, (float, int, np.ndarray)): + raise TypeError("Expecting the given stiffness gain kp to be an int, float, np.array, instead got: " + "{}".format(type(kp))) + if isinstance(kp, np.ndarray) and kp.shape != (self.x_size, self.x_size): + raise ValueError("Expecting the given stiffness gain matrix kp to be of shape {}, but instead got " + "shape: {}".format((self.x_size, self.x_size), kp.shape)) + self._kp = kp + + ########### + # Methods # + ########### + + def set_desired_references(self, x_des, dx_des=None, *args, **kwargs): + """Set the desired references. + + Args: + x_des (np.array[N], None): desired joint positions, where :math:`N` is the number of DoFs. If None, + it will be set to 0. + dx_des (np.array[N], None): desired joint velocities, where :math:`N` is the number of DoFs. If None, + it will be set to 0. + """ + self.x_desired = x_des + self.dx_desired = dx_des + + def get_desired_references(self): + """Return the desired references. + + Returns: + np.array[N]: desired joint positions. + np.array[N]: desired joint velocities. + """ + return self.x_desired, self.dx_desired + + def _update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + q = self.model.get_joint_positions() + + # update b vector + self._b = np.dot(self.kp, (self._q_d - q)) + self._dq_d # shape: (N,) diff --git a/pyrobolearn/priorities/tasks/velocity/pure_rolling.py b/pyrobolearn/priorities/tasks/velocity/pure_rolling.py new file mode 100644 index 0000000..9badc1a --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/pure_rolling.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python +r"""Provide the pure rolling (no sliding) task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + - [2] "On the Kinematics of Wheeled Motion Control of a Hybrid Wheeled-Legged CENTAURO Robot", Kamedula et al., 2019 +""" + +# TODO: finish to implement this + +import numpy as np + +from pyrobolearn.priorities.tasks import JointVelocityTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Arturo Laurenzi (C++)", "Malgorzata Kamedula (insight)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class PureRollingTask(JointVelocityTask): + r"""Pure rolling (no-sliding) Task + + The pure rolling without slipping constrained the end-effector contact point (with the ground) to remain fixed: + + .. math:: v_{cp} = 0 + + where :math:`v_{cp}` is the contact point velocity given by :math:`v_{cp} = \dot{x}_{cp}` where :math:`x_{cp}` is + the position vector (from the world frame origin) to the contact point. + + The optimization problem can be formulated as: + + .. math:: || S J_c(q) \dot{q} ||^2 + + where :math:`S = [0_{3 \times 3}, 1_{3 \times 3}]` is a selector matrix that selects the cartesian linear + velocities from :math:`J_c(q)`, which is the Jacobian of the contact point between the wheel and + the ground (in the world frame), and :math:`\dot{q}` are the joint velocities being optimized. + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + """ + + def __init__(self, model, weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + weight (float, np.array[3,3]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(PureRollingTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # TODO: check that the model is a wheeled robot + + raise NotImplementedError("This class has not been implemented yet.") diff --git a/pyrobolearn/priorities/tasks/velocity/rigid_rotation.py b/pyrobolearn/priorities/tasks/velocity/rigid_rotation.py new file mode 100644 index 0000000..9b55cf9 --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/rigid_rotation.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python +r"""Provide the rigid rotation task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +# TODO: finish to implement this task + +import numpy as np + +from pyrobolearn.priorities.tasks import JointVelocityTask + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Arturo Laurenzi (C++)", "Malgorzata Kamedula (insight)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class RigidRotationTask(JointVelocityTask): + r"""Rigid Rotation Task + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + """ + + def __init__(self, model, weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface. + weight (float, np.array[N,N]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(RigidRotationTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # TODO: check that the model is a wheeled robot + + raise NotImplementedError("This class has not been implemented yet.") diff --git a/pyrobolearn/priorities/tasks/velocity/unicycle.py b/pyrobolearn/priorities/tasks/velocity/unicycle.py new file mode 100644 index 0000000..c88955b --- /dev/null +++ b/pyrobolearn/priorities/tasks/velocity/unicycle.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python +r"""Provide the unicycle task. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + +References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 +""" + +# TODO: finish to implement this class + +import numpy as np + +from pyrobolearn.priorities.tasks import Task + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Enrico Mingo Hoffman (C++)", "Juan Alejandro Castano (C++)", "Brian Delhaisse (Python + doc)"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class UnicycleTask(Task): + r"""Unicycle Task + + The unicycle task defines a rotation around fix axes to allow the robot wheels to spin. It basically creates a + new Cartesian tasks at the wheels. + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + """ + + def __init__(self, model, weight=1., constraints=[]): + """ + Initialize the task. + + Args: + model (ModelInterface): model interface + weight (float, np.array[N,N]): weight scalar or matrix associated to the task. + constraints (list of Constraint): list of constraints associated with the task. + """ + super(UnicycleTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # TODO: check that the model is a wheeled robot + + raise NotImplementedError("This class has not been implemented yet.")