From c096ef19888ae98448e93a43d79faf38052667c2 Mon Sep 17 00:00:00 2001 From: Giuseppe L'Erario Date: Thu, 14 Dec 2023 10:16:07 +0100 Subject: [PATCH 01/20] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 72dc08f3..dbc4d7fd 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ **Automatic Differentiation for rigid-body-dynamics AlgorithMs** -ADAM implements a collection of algorithms for calculating rigid-body dynamics for **floating-base** robots, in _mixed_ and _base fixed representations_, also called _left trivialized_ representation (see [Traversaro's A Unified View of the Equations of Motion used for Control Design of Humanoid Robots](https://www.researchgate.net/publication/312200239_A_Unified_View_of_the_Equations_of_Motion_used_for_Control_Design_of_Humanoid_Robots)) using: +ADAM implements a collection of algorithms for calculating rigid-body dynamics for **floating-base** robots, in _mixed_ and _base fixed representations_ (see [Traversaro's A Unified View of the Equations of Motion used for Control Design of Humanoid Robots](https://www.researchgate.net/publication/312200239_A_Unified_View_of_the_Equations_of_Motion_used_for_Control_Design_of_Humanoid_Robots)) using: - [Jax](https://github.com/google/jax) - [CasADi](https://web.casadi.org/) From eb722cd18b6c367e6296007b62f6a2d6805aa0ee Mon Sep 17 00:00:00 2001 From: Giuseppe L'Erario Date: Thu, 14 Dec 2023 10:16:41 +0100 Subject: [PATCH 02/20] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index dbc4d7fd..b1a103ee 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ **Automatic Differentiation for rigid-body-dynamics AlgorithMs** -ADAM implements a collection of algorithms for calculating rigid-body dynamics for **floating-base** robots, in _mixed_ and _base fixed representations_ (see [Traversaro's A Unified View of the Equations of Motion used for Control Design of Humanoid Robots](https://www.researchgate.net/publication/312200239_A_Unified_View_of_the_Equations_of_Motion_used_for_Control_Design_of_Humanoid_Robots)) using: +ADAM implements a collection of algorithms for calculating rigid-body dynamics for **floating-base** robots, in _mixed_ and _body fixed representations_ (see [Traversaro's A Unified View of the Equations of Motion used for Control Design of Humanoid Robots](https://www.researchgate.net/publication/312200239_A_Unified_View_of_the_Equations_of_Motion_used_for_Control_Design_of_Humanoid_Robots)) using: - [Jax](https://github.com/google/jax) - [CasADi](https://web.casadi.org/) From 67d0cd12a12cd6c84ef6dc57540e69d8601b2c7d Mon Sep 17 00:00:00 2001 From: Giuseppe L'Erario Date: Thu, 14 Dec 2023 10:40:14 +0100 Subject: [PATCH 03/20] Suppress ipopt output --- examples/mpc-ik.ipynb | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/examples/mpc-ik.ipynb b/examples/mpc-ik.ipynb index 6b2273d8..25c6a16e 100644 --- a/examples/mpc-ik.ipynb +++ b/examples/mpc-ik.ipynb @@ -7,7 +7,7 @@ "colab_type": "text" }, "source": [ - "\"Open" + "\"Open" ] }, { @@ -232,7 +232,7 @@ "opti.minimize(target_cost + velocities_cost)\n", "\n", "# set the solver\n", - "p_opts = {\"expand\": True}\n", + "p_opts = {\"expand\": True, 'ipopt.print_level': 0, 'print_time': 0, 'ipopt.sb': 'yes'}\n", "s_opts = {\"max_iter\": 100, \"print_level\": 0}\n", "opti.solver(\"ipopt\", p_opts, s_opts)\n" ] @@ -260,6 +260,8 @@ }, "outputs": [], "source": [ + "print(\"Simulation in progress. Wait a bit!\")\n", + "\n", "# start mujoco simulation along with control\n", "duration = 10 # (seconds)\n", "framerate = 60 # (Hz)\n", From afac51296ab911c71fbfade7edba1dfa5797e5cb Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Fri, 15 Dec 2023 12:10:14 +0100 Subject: [PATCH 04/20] Avoid to print a warning for each sensor tag in loaded URDF file --- src/adam/model/std_factories/std_model.py | 25 ++++++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/src/adam/model/std_factories/std_model.py b/src/adam/model/std_factories/std_model.py index e7da13da..87f9310a 100644 --- a/src/adam/model/std_factories/std_model.py +++ b/src/adam/model/std_factories/std_model.py @@ -1,11 +1,25 @@ import pathlib from typing import List +import xml.etree.ElementTree as ET import urdf_parser_py.urdf from adam.core.spatial_math import SpatialMath from adam.model import ModelFactory, StdJoint, StdLink +def urdf_remove_sensors_tags(xml_string): + # Parse the XML string + root = ET.fromstring(xml_string) + + # Find and remove all tags named "sensor" that are child of + # root node (i.e. robot) + for sensors_tag in root.findall("sensor"): + root.remove(sensors_tag) + + # Convert the modified XML back to a string + modified_xml_string = ET.tostring(root) + + return modified_xml_string class URDFModelFactory(ModelFactory): """This factory generates robot elements from urdf_parser_py @@ -21,7 +35,16 @@ def __init__(self, path: str, math: SpatialMath): if not path.exists(): raise FileExistsError(path) - self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_file(path) + # Read URDF, but before passing it to urdf_parser_py get rid of all sensor tags + # sensor tags are valid elements of URDF (see ), + # but they are ignored by urdf_parser_py, that complains every time it sees one. + # As there is nothing to be fixed in the used models, and it is not useful + # to have a useless and noisy warning, let's remove before hands all the sensor elements, + # that anyhow are not parser by urdf_parser_py or adam + # See https://github.com/ami-iit/ADAM/issues/59 + xml_string = open(path, 'r').read() + xml_string_without_sensors_tags = urdf_remove_sensors_tags(xml_string) + self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_string(xml_string_without_sensors_tags) self.name = self.urdf_desc.name def get_joints(self) -> List[StdJoint]: From e60db393467de36eebb2aa18789de8ae81a5bc6e Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Fri, 15 Dec 2023 12:17:41 +0100 Subject: [PATCH 05/20] Fix missing close of file --- src/adam/model/std_factories/std_model.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/adam/model/std_factories/std_model.py b/src/adam/model/std_factories/std_model.py index 87f9310a..41d8af69 100644 --- a/src/adam/model/std_factories/std_model.py +++ b/src/adam/model/std_factories/std_model.py @@ -42,7 +42,9 @@ def __init__(self, path: str, math: SpatialMath): # to have a useless and noisy warning, let's remove before hands all the sensor elements, # that anyhow are not parser by urdf_parser_py or adam # See https://github.com/ami-iit/ADAM/issues/59 - xml_string = open(path, 'r').read() + xml_file = open(path, 'r') + xml_string = xml_file.read() + xml_file.close() xml_string_without_sensors_tags = urdf_remove_sensors_tags(xml_string) self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_string(xml_string_without_sensors_tags) self.name = self.urdf_desc.name From 5693e13a9440e1b283dd2172524b6fa893be6c22 Mon Sep 17 00:00:00 2001 From: Giuseppe L'Erario Date: Fri, 15 Dec 2023 12:31:59 +0100 Subject: [PATCH 06/20] Update README.md changing the capitalization (ADAM -> adam) --- README.md | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index b1a103ee..c9b2226c 100644 --- a/README.md +++ b/README.md @@ -1,20 +1,20 @@ -# ADAM +# adam -[![Adam](https://github.com/ami-iit/ADAM/actions/workflows/tests.yml/badge.svg?branch=main)](https://github.com/ami-iit/ADAM/actions/workflows/tests.yml) +[![adam](https://github.com/ami-iit/ADAM/actions/workflows/tests.yml/badge.svg?branch=main)](https://github.com/ami-iit/ADAM/actions/workflows/tests.yml) [![](https://img.shields.io/badge/license-LGPL-19c2d8.svg)](https://github.com/ami-iit/ADAM/blob/main/LICENSE) **Automatic Differentiation for rigid-body-dynamics AlgorithMs** -ADAM implements a collection of algorithms for calculating rigid-body dynamics for **floating-base** robots, in _mixed_ and _body fixed representations_ (see [Traversaro's A Unified View of the Equations of Motion used for Control Design of Humanoid Robots](https://www.researchgate.net/publication/312200239_A_Unified_View_of_the_Equations_of_Motion_used_for_Control_Design_of_Humanoid_Robots)) using: +**adam** implements a collection of algorithms for calculating rigid-body dynamics for **floating-base** robots, in _mixed_ and _body fixed representations_ (see [Traversaro's A Unified View of the Equations of Motion used for Control Design of Humanoid Robots](https://www.researchgate.net/publication/312200239_A_Unified_View_of_the_Equations_of_Motion_used_for_Control_Design_of_Humanoid_Robots)) using: - [Jax](https://github.com/google/jax) - [CasADi](https://web.casadi.org/) - [PyTorch](https://github.com/pytorch/pytorch) - [NumPy](https://numpy.org/) -ADAM employs the **automatic differentiation** capabilities of these frameworks to compute, if needed, gradients, Jacobian, Hessians of rigid-body dynamics quantities. This approach enables the design of optimal control and reinforcement learning strategies in robotics. +**adam** employs the **automatic differentiation** capabilities of these frameworks to compute, if needed, gradients, Jacobian, Hessians of rigid-body dynamics quantities. This approach enables the design of optimal control and reinforcement learning strategies in robotics. -ADAM is based on Roy Featherstone's Rigid Body Dynamics Algorithms. +**adam** is based on Roy Featherstone's Rigid Body Dynamics Algorithms. --- @@ -94,8 +94,8 @@ pip install adam-robotics[selected-interface]@git+https://github.com/ami-iit/ADA or clone the repo and install: ```bash -git clone https://github.com/ami-iit/ADAM.git -cd ADAM +git clone https://github.com/ami-iit/adam.git +cd adam pip install .[selected-interface] ``` @@ -142,13 +142,13 @@ Activate the environment, clone the repo and install the library: ```bash mamba activate adamenv git clone https://github.com/ami-iit/ADAM.git -cd ADAM +cd adam pip install --no-deps . ``` ## πŸš€ Usage -The following are small snippets of the use of ADAM. More examples are arriving! +The following are small snippets of the use of **adam**. More examples are arriving! Have also a look at te `tests` folder. ### Jax interface @@ -246,7 +246,7 @@ print(M) ## πŸ¦Έβ€β™‚οΈ Contributing -**ADAM** is an open-source project. Contributions are very welcome! +**adam** is an open-source project. Contributions are very welcome! Open an issue with your feature request or if you spot a bug. Then, you can also proceed with a Pull-requests! :rocket: From 94ed370c2419767b5b9b4ce1bb191772337346ca Mon Sep 17 00:00:00 2001 From: Giuseppe L'Erario Date: Fri, 12 Jan 2024 13:01:48 +0100 Subject: [PATCH 07/20] Update black.yml --- .github/workflows/black.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/black.yml b/.github/workflows/black.yml index 8ba32be4..ca64506b 100644 --- a/.github/workflows/black.yml +++ b/.github/workflows/black.yml @@ -11,7 +11,7 @@ jobs: steps: - uses: actions/checkout@v2 - name: black - uses: lgeiger/black-action@v1.0.1 + uses: psf/black@stable with: args: . - name: Check for modified files From 8c26f1e8fbbf3e91c2fa6b7cba17a9cbd05186e1 Mon Sep 17 00:00:00 2001 From: Giuseppe L'Erario Date: Fri, 12 Jan 2024 13:06:24 +0100 Subject: [PATCH 08/20] Add on pull_request --- .github/workflows/black.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/black.yml b/.github/workflows/black.yml index ca64506b..52d61d27 100644 --- a/.github/workflows/black.yml +++ b/.github/workflows/black.yml @@ -1,6 +1,7 @@ name: Black action on: + pull_request: push: branches: - main From 349fbc649b334d6050e347c1502ea0b42638fdac Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 12 Jan 2024 13:13:55 +0100 Subject: [PATCH 09/20] Align with black formatting --- src/adam/model/std_factories/std_model.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/adam/model/std_factories/std_model.py b/src/adam/model/std_factories/std_model.py index 41d8af69..ffeff713 100644 --- a/src/adam/model/std_factories/std_model.py +++ b/src/adam/model/std_factories/std_model.py @@ -7,11 +7,12 @@ from adam.core.spatial_math import SpatialMath from adam.model import ModelFactory, StdJoint, StdLink + def urdf_remove_sensors_tags(xml_string): # Parse the XML string root = ET.fromstring(xml_string) - # Find and remove all tags named "sensor" that are child of + # Find and remove all tags named "sensor" that are child of # root node (i.e. robot) for sensors_tag in root.findall("sensor"): root.remove(sensors_tag) @@ -21,6 +22,7 @@ def urdf_remove_sensors_tags(xml_string): return modified_xml_string + class URDFModelFactory(ModelFactory): """This factory generates robot elements from urdf_parser_py @@ -36,17 +38,19 @@ def __init__(self, path: str, math: SpatialMath): raise FileExistsError(path) # Read URDF, but before passing it to urdf_parser_py get rid of all sensor tags - # sensor tags are valid elements of URDF (see ), + # sensor tags are valid elements of URDF (see ), # but they are ignored by urdf_parser_py, that complains every time it sees one. # As there is nothing to be fixed in the used models, and it is not useful # to have a useless and noisy warning, let's remove before hands all the sensor elements, # that anyhow are not parser by urdf_parser_py or adam # See https://github.com/ami-iit/ADAM/issues/59 - xml_file = open(path, 'r') + xml_file = open(path, "r") xml_string = xml_file.read() xml_file.close() xml_string_without_sensors_tags = urdf_remove_sensors_tags(xml_string) - self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_string(xml_string_without_sensors_tags) + self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_string( + xml_string_without_sensors_tags + ) self.name = self.urdf_desc.name def get_joints(self) -> List[StdJoint]: From 7f1133d3194c4e572a8c72c3047e88104756a9ef Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 12 Jan 2024 13:15:40 +0100 Subject: [PATCH 10/20] Update actions/checkout version in black.yml workflow --- .github/workflows/black.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/black.yml b/.github/workflows/black.yml index 52d61d27..be355329 100644 --- a/.github/workflows/black.yml +++ b/.github/workflows/black.yml @@ -10,7 +10,7 @@ jobs: lint: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - name: black uses: psf/black@stable with: From 31e567da9322bb2f4981763004febc8ce8b858d2 Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 12 Jan 2024 13:18:41 +0100 Subject: [PATCH 11/20] Remove unnecessary 'args' parameter in black workflow --- .github/workflows/black.yml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.github/workflows/black.yml b/.github/workflows/black.yml index be355329..83b92fa3 100644 --- a/.github/workflows/black.yml +++ b/.github/workflows/black.yml @@ -13,8 +13,6 @@ jobs: - uses: actions/checkout@v3 - name: black uses: psf/black@stable - with: - args: . - name: Check for modified files id: git-check run: echo ::set-output name=modified::$(if git diff-index --quiet HEAD --; then echo "false"; else echo "true"; fi) From 29c985c54530af543abf66615ef1776af286c53e Mon Sep 17 00:00:00 2001 From: Giuseppe L'Erario Date: Fri, 12 Jan 2024 15:03:15 +0100 Subject: [PATCH 12/20] Update .github/workflows/black.yml Co-authored-by: Filippo Luca Ferretti <102977828+flferretti@users.noreply.github.com> --- .github/workflows/black.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/black.yml b/.github/workflows/black.yml index 83b92fa3..41be665b 100644 --- a/.github/workflows/black.yml +++ b/.github/workflows/black.yml @@ -13,6 +13,8 @@ jobs: - uses: actions/checkout@v3 - name: black uses: psf/black@stable + with: + options: "--check --verbose" - name: Check for modified files id: git-check run: echo ::set-output name=modified::$(if git diff-index --quiet HEAD --; then echo "false"; else echo "true"; fi) From 2a1d44d6f39a76a5097c189b040af585370e8901 Mon Sep 17 00:00:00 2001 From: Giuseppe L'Erario Date: Thu, 18 Jan 2024 15:33:56 +0100 Subject: [PATCH 13/20] Update LICENSE --- LICENSE | 532 +++----------------------------------------------------- 1 file changed, 28 insertions(+), 504 deletions(-) diff --git a/LICENSE b/LICENSE index 8000a6fa..aba21cca 100644 --- a/LICENSE +++ b/LICENSE @@ -1,504 +1,28 @@ - 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. - - GNU LESSER GENERAL PUBLIC LICENSE - 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 - - How to Apply These Terms to Your New Libraries - - If you develop a new library, and you want it to be of the greatest -possible use to the public, we recommend making it free software that -everyone can redistribute and change. You can do so by permitting -redistribution under these terms (or, alternatively, under the terms of the -ordinary General Public License). - - To apply these terms, attach the following notices to the library. It is -safest to attach them to the start of each source file to most effectively -convey the exclusion of warranty; and each file should have at least the -"copyright" line and a pointer to where the full notice is found. - - - Copyright (C) - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 - USA - -Also add information on how to contact you by electronic and paper mail. - -You should also get your employer (if you work as a programmer) or your -school, if any, to sign a "copyright disclaimer" for the library, if -necessary. Here is a sample; alter the names: - - Yoyodyne, Inc., hereby disclaims all copyright interest in the - library `Frob' (a library for tweaking knobs) written by James Random - Hacker. - - , 1 April 1990 - Ty Coon, President of Vice - -That's all there is to it! +BSD 3-Clause License + +Copyright (c) 2024, Fondazione Istituto Italiano di Tecnologia + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. From cd5536014ea58c159a398b54f9467f6d8af0dc89 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 25 Jan 2024 18:15:33 +0100 Subject: [PATCH 14/20] Convert array to double precision in the creation and update tensor operations --- src/adam/pytorch/torch_like.py | 45 ++++++++++++++++++++-------------- 1 file changed, 27 insertions(+), 18 deletions(-) diff --git a/src/adam/pytorch/torch_like.py b/src/adam/pytorch/torch_like.py index f24d91c5..0bc98ea1 100644 --- a/src/adam/pytorch/torch_like.py +++ b/src/adam/pytorch/torch_like.py @@ -7,9 +7,9 @@ import numpy.typing as ntp import torch +import numpy as np from adam.core.spatial_math import ArrayLike, ArrayLikeFactory, SpatialMath -from adam.numpy import NumpyLike @dataclass @@ -18,12 +18,17 @@ class TorchLike(ArrayLike): array: torch.Tensor + def __post_init__(self): + """Converts array to double precision""" + if self.array.dtype != torch.float64: + self.array = self.array.double() + def __setitem__(self, idx, value: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": """Overrides set item operator""" if type(self) is type(value): self.array[idx] = value.array.reshape(self.array[idx].shape) else: - self.array[idx] = torch.tensor(value) + self.array[idx] = torch.tensor(value) if isinstance(value, float) else value def __getitem__(self, idx): """Overrides get item operator""" @@ -42,24 +47,27 @@ def T(self) -> "TorchLike": Returns: TorchLike: transpose of array """ - if len(self.array.shape) != 1: - return TorchLike(self.array.mT) + # check if self.array is a 0-D tensor + + if len(self.array.shape) == 0: + return TorchLike(self.array) x = self.array return TorchLike(x.permute(*torch.arange(x.ndim - 1, -1, -1))) def __matmul__(self, other: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": """Overrides @ operator""" + if type(self) is type(other): - return TorchLike(self.array @ other.array.float()) + return TorchLike(self.array @ other.array) else: - return TorchLike(self.array @ torch.tensor(other).float()) + return TorchLike(self.array @ torch.tensor(other)) def __rmatmul__(self, other: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": """Overrides @ operator""" if type(self) is type(other): return TorchLike(other.array @ self.array) else: - return TorchLike(torch.tensor(other).float() @ self.array) + return TorchLike(torch.tensor(other) @ self.array) def __mul__(self, other: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": """Overrides * operator""" @@ -120,7 +128,7 @@ def zeros(*x: int) -> "TorchLike": Returns: TorchLike: zero matrix of dimension *x """ - return TorchLike(torch.zeros(x).float()) + return TorchLike(torch.zeros(x)) @staticmethod def eye(x: int) -> "TorchLike": @@ -131,7 +139,7 @@ def eye(x: int) -> "TorchLike": Returns: TorchLike: identity matrix of dimension x """ - return TorchLike(torch.eye(x).float()) + return TorchLike(torch.eye(x)) @staticmethod def array(x: ntp.ArrayLike) -> "TorchLike": @@ -139,7 +147,7 @@ def array(x: ntp.ArrayLike) -> "TorchLike": Returns: TorchLike: vector wrapping x """ - return TorchLike(torch.FloatTensor(x)) + return TorchLike(torch.tensor(x)) class SpatialMath(SpatialMath): @@ -155,7 +163,8 @@ def sin(x: ntp.ArrayLike) -> "TorchLike": Returns: TorchLike: sin value of x """ - x = torch.tensor(x) + if isinstance(x, float): + x = torch.tensor(x) return TorchLike(torch.sin(x)) @staticmethod @@ -167,7 +176,9 @@ def cos(x: ntp.ArrayLike) -> "TorchLike": Returns: TorchLike: cos value of x """ - x = torch.tensor(x) + # transform to torch tensor, if not already + if isinstance(x, float): + x = torch.tensor(x) return TorchLike(torch.cos(x)) @staticmethod @@ -193,13 +204,11 @@ def skew(x: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": """ if not isinstance(x, TorchLike): return TorchLike( - torch.FloatTensor( - [[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]] - ) + torch.tensor([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]]) ) x = x.array return TorchLike( - torch.FloatTensor([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]]) + torch.tensor([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]]) ) @staticmethod @@ -211,7 +220,7 @@ def vertcat(*x: ntp.ArrayLike) -> "TorchLike": if isinstance(x[0], TorchLike): v = torch.vstack([x[i].array for i in range(len(x))]) else: - v = torch.FloatTensor(x) + v = torch.tensor(x) return TorchLike(v) @staticmethod @@ -223,5 +232,5 @@ def horzcat(*x: ntp.ArrayLike) -> "TorchLike": if isinstance(x[0], TorchLike): v = torch.hstack([x[i].array for i in range(len(x))]) else: - v = torch.FloatTensor(x) + v = torch.tensor(x) return TorchLike(v) From 5de643305f51d38e2d025e1ad871aaf37e93acc6 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 25 Jan 2024 18:15:52 +0100 Subject: [PATCH 15/20] Update gravity initialization to float64 in KinDynComputationsParametric and KinDynComputations --- .../parametric/pytorch/computations_parametric.py | 14 +++++++++----- src/adam/pytorch/computations.py | 10 ++++++---- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/src/adam/parametric/pytorch/computations_parametric.py b/src/adam/parametric/pytorch/computations_parametric.py index 8e53eea1..08f9fb55 100644 --- a/src/adam/parametric/pytorch/computations_parametric.py +++ b/src/adam/parametric/pytorch/computations_parametric.py @@ -23,7 +23,9 @@ def __init__( joints_name_list: list, links_name_list: list, root_link: str = "root_link", - gravity: np.array = torch.FloatTensor([0, 0, -9.80665, 0, 0, 0]), + gravity: np.array = torch.tensor( + [0, 0, -9.80665, 0, 0, 0], dtype=torch.float64 + ), ) -> None: """ Args: @@ -119,7 +121,7 @@ def forward_kinematics( self, frame, base_transform: torch.Tensor, - s: torch.Tensor, + joint_postitions: torch.Tensor, length_multiplier: torch.Tensor, densities: torch.Tensor, ) -> torch.Tensor: @@ -128,7 +130,7 @@ def forward_kinematics( Args: frame (str): The frame to which the fk will be computed base_transform (torch.tensor): The homogenous transform from base to world frame - s (torch.tensor): The joints position + joints_postition (torch.tensor): The joints position length_multiplier (torch.tensor): The length multiplier of the parametrized links densities (torch.tensor): The densities of the parametrized links @@ -149,7 +151,9 @@ def forward_kinematics( self.NDoF = self.rbdalgos.NDoF return ( self.rbdalgos.forward_kinematics( - frame, torch.FloatTensor(base_transform), torch.FloatTensor(s) + frame, + base_transform, + joint_postitions, ) ).array @@ -414,7 +418,7 @@ def gravity_term( base_positions, torch.zeros(6).reshape(6, 1), torch.zeros(self.NDoF), - torch.FloatTensor(self.g), + self.g, ).array.squeeze() def get_total_mass( diff --git a/src/adam/pytorch/computations.py b/src/adam/pytorch/computations.py index f614acf9..e81c48be 100644 --- a/src/adam/pytorch/computations.py +++ b/src/adam/pytorch/computations.py @@ -21,7 +21,9 @@ def __init__( urdfstring: str, joints_name_list: list = None, root_link: str = "root_link", - gravity: np.array = torch.FloatTensor([0, 0, -9.80665, 0, 0, 0]), + gravity: np.array = torch.tensor( + [0, 0, -9.80665, 0, 0, 0], dtype=torch.float64 + ), ) -> None: """ Args: @@ -92,8 +94,8 @@ def forward_kinematics( return ( self.rbdalgos.forward_kinematics( frame, - torch.FloatTensor(base_transform), - torch.FloatTensor(joint_position), + base_transform, + joint_position, ) ).array @@ -240,7 +242,7 @@ def gravity_term( joint_positions, torch.zeros(6).reshape(6, 1), torch.zeros(self.NDoF), - torch.FloatTensor(self.g), + self.g, ).array.squeeze() def get_total_mass(self) -> float: From f308a52987ed1d758b54167d8e74b39841f2bedd Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 25 Jan 2024 18:16:12 +0100 Subject: [PATCH 16/20] Update PyTorch tests --- tests/body_fixed/test_pytorch_body_fixed.py | 42 ++++++++---- tests/mixed/test_pytorch_mixed.py | 42 ++++++++---- .../test_pytorch_computations_parametric.py | 68 +++++++++---------- 3 files changed, 88 insertions(+), 64 deletions(-) diff --git a/tests/body_fixed/test_pytorch_body_fixed.py b/tests/body_fixed/test_pytorch_body_fixed.py index b0df67bb..daa3a142 100644 --- a/tests/body_fixed/test_pytorch_body_fixed.py +++ b/tests/body_fixed/test_pytorch_body_fixed.py @@ -13,6 +13,7 @@ from adam import Representations from adam.geometry import utils from adam.pytorch import KinDynComputations +from adam.pytorch.torch_like import SpatialMath np.random.seed(42) torch.set_default_dtype(torch.float64) @@ -71,17 +72,18 @@ def H_from_Pos_RPY_idyn(xyz, rpy): kinDyn.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) n_dofs = len(joints_name_list) -# base pose quantities -xyz = (np.random.rand(3) - 0.5) * 5 -rpy = (np.random.rand(3) - 0.5) * 5 -base_vel = (np.random.rand(6) - 0.5) * 5 +xyz = (torch.rand(3) - 0.5) * 5 +rpy = (torch.rand(3) - 0.5) * 5 +base_vel = (torch.rand(6) - 0.5) * 5 # joints quantitites -joints_val = (np.random.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 +joints_val = (torch.rand(n_dofs) - 0.5) * 5 +joints_dot_val = (torch.rand(n_dofs) - 0.5) * 5 -g = np.array([0, 0, -9.80665]) -H_b = utils.H_from_Pos_RPY(xyz, rpy) -kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) +g = torch.tensor([0, 0, -9.80665]) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array +kinDyn.setRobotState( + H_b.numpy(), joints_val.numpy(), base_vel.numpy(), joints_dot_val.numpy(), g.numpy() +) def test_mass_matrix(): @@ -165,8 +167,14 @@ def test_bias_force(): def test_coriolis_term(): - g0 = np.zeros(3) - kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) + g0 = torch.zeros(3) + kinDyn.setRobotState( + H_b.numpy(), + joints_val.numpy(), + base_vel.numpy(), + joints_dot_val.numpy(), + g0.numpy(), + ) C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) assert kinDyn.generalizedBiasForces(C_iDyn) C_iDyn_np = np.concatenate( @@ -181,9 +189,15 @@ def test_gravity_term(): kinDyn2.loadRobotModel(robot_iDyn.model()) kinDyn2.setFloatingBase(root_link) kinDyn2.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) - base_vel0 = np.zeros(6) - joints_dot_val0 = np.zeros(n_dofs) - kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) + base_vel0 = torch.zeros(6) + joints_dot_val0 = torch.zeros(n_dofs) + kinDyn2.setRobotState( + H_b.numpy(), + joints_val.numpy(), + base_vel0.numpy(), + joints_dot_val0.numpy(), + g.numpy(), + ) G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) assert kinDyn2.generalizedBiasForces(G_iDyn) G_iDyn_np = np.concatenate( diff --git a/tests/mixed/test_pytorch_mixed.py b/tests/mixed/test_pytorch_mixed.py index de6a8229..32a72702 100644 --- a/tests/mixed/test_pytorch_mixed.py +++ b/tests/mixed/test_pytorch_mixed.py @@ -13,6 +13,7 @@ from adam import Representations from adam.geometry import utils from adam.pytorch import KinDynComputations +from adam.pytorch.torch_like import SpatialMath np.random.seed(42) torch.set_default_dtype(torch.float64) @@ -71,17 +72,18 @@ def H_from_Pos_RPY_idyn(xyz, rpy): kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) n_dofs = len(joints_name_list) -# base pose quantities -xyz = (np.random.rand(3) - 0.5) * 5 -rpy = (np.random.rand(3) - 0.5) * 5 -base_vel = (np.random.rand(6) - 0.5) * 5 +xyz = (torch.rand(3) - 0.5) * 5 +rpy = (torch.rand(3) - 0.5) * 5 +base_vel = (torch.rand(6) - 0.5) * 5 # joints quantitites -joints_val = (np.random.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 +joints_val = (torch.rand(n_dofs) - 0.5) * 5 +joints_dot_val = (torch.rand(n_dofs) - 0.5) * 5 -g = np.array([0, 0, -9.80665]) -H_b = utils.H_from_Pos_RPY(xyz, rpy) -kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) +g = torch.tensor([0, 0, -9.80665]) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array +kinDyn.setRobotState( + H_b.numpy(), joints_val.numpy(), base_vel.numpy(), joints_dot_val.numpy(), g.numpy() +) def test_mass_matrix(): @@ -165,8 +167,14 @@ def test_bias_force(): def test_coriolis_term(): - g0 = np.zeros(3) - kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) + g0 = torch.zeros(3) + kinDyn.setRobotState( + H_b.numpy(), + joints_val.numpy(), + base_vel.numpy(), + joints_dot_val.numpy(), + g0.numpy(), + ) C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) assert kinDyn.generalizedBiasForces(C_iDyn) C_iDyn_np = np.concatenate( @@ -181,9 +189,15 @@ def test_gravity_term(): kinDyn2.loadRobotModel(robot_iDyn.model()) kinDyn2.setFloatingBase(root_link) kinDyn2.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) - base_vel0 = np.zeros(6) - joints_dot_val0 = np.zeros(n_dofs) - kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) + base_vel0 = torch.zeros(6) + joints_dot_val0 = torch.zeros(n_dofs) + kinDyn2.setRobotState( + H_b.numpy(), + joints_val.numpy(), + base_vel0.numpy(), + joints_dot_val0.numpy(), + g.numpy(), + ) G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) assert kinDyn2.generalizedBiasForces(G_iDyn) G_iDyn_np = np.concatenate( diff --git a/tests/parametric/test_pytorch_computations_parametric.py b/tests/parametric/test_pytorch_computations_parametric.py index f40a89ee..ba51df96 100644 --- a/tests/parametric/test_pytorch_computations_parametric.py +++ b/tests/parametric/test_pytorch_computations_parametric.py @@ -11,6 +11,7 @@ import numpy as np from adam.parametric.pytorch import KinDynComputationsParametric from adam.pytorch import KinDynComputations +from adam.pytorch.torch_like import SpatialMath from adam.geometry import utils import tempfile @@ -61,11 +62,6 @@ "r_ankle_roll", ] - -def SX2DM(x): - return cs.DM(x) - - logging.basicConfig(level=logging.DEBUG) logging.debug("Showing the robot tree.") @@ -77,8 +73,6 @@ def SX2DM(x): comp_w_hardware = KinDynComputationsParametric( model_path, joints_name_list, link_name_list, root_link ) -# comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) -# comp_w_hardware.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) original_density = [ 628.0724496264945 @@ -87,41 +81,39 @@ def SX2DM(x): n_dofs = len(joints_name_list) # base pose quantities -xyz = (np.random.rand(3) - 0.5) * 5 -rpy = (np.random.rand(3) - 0.5) * 5 -base_vel = (np.random.rand(6) - 0.5) * 5 +xyz = (torch.rand(3) - 0.5) * 5 +rpy = (torch.rand(3) - 0.5) * 5 +base_vel = (torch.rand(6) - 0.5) * 5 # joints quantitites -joints_val = (np.random.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 +joints_val = (torch.rand(n_dofs) - 0.5) * 5 +joints_dot_val = (torch.rand(n_dofs) - 0.5) * 5 -H_b = torch.FloatTensor(utils.H_from_Pos_RPY(xyz, rpy)) -vb_ = torch.FloatTensor(base_vel) -s_ = torch.FloatTensor(joints_val) -s_dot_ = torch.FloatTensor(joints_dot_val) +g = torch.tensor([0, 0, -9.80665]) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array def test_mass_matrix(): - mass_test = comp.mass_matrix(H_b, s_) + mass_test = comp.mass_matrix(H_b, joints_val) mass_test_hardware = np.array( - comp_w_hardware.mass_matrix(H_b, s_, original_length, original_density) + comp_w_hardware.mass_matrix(H_b, joints_val, original_length, original_density) ) assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5) def test_CMM(): - Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) + Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) Jcm_test_hardware = np.array( comp_w_hardware.centroidal_momentum_matrix( - H_b, s_, original_length, original_density + H_b, joints_val, original_length, original_density ) ) assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5) def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, s_) + CoM_test = comp.CoM_position(H_b, joints_val) CoM_hardware = np.array( - comp_w_hardware.CoM_position(H_b, s_, original_length, original_density) + comp_w_hardware.CoM_position(H_b, joints_val, original_length, original_density) ) assert CoM_test - CoM_hardware == pytest.approx(0.0, abs=1e-5) @@ -133,17 +125,21 @@ def test_total_mass(): def test_jacobian(): - J_test = comp.jacobian("l_sole", H_b, s_) + J_test = comp.jacobian("l_sole", H_b, joints_val) J_test_hardware = np.array( - comp_w_hardware.jacobian("l_sole", H_b, s_, original_length, original_density) + comp_w_hardware.jacobian( + "l_sole", H_b, joints_val, original_length, original_density + ) ) assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) def test_jacobian_non_actuated(): - J_test = comp.jacobian("head", H_b, s_) + J_test = comp.jacobian("head", H_b, joints_val) J_test_hardware = np.array( - comp_w_hardware.jacobian("head", H_b, s_, original_length, original_density) + comp_w_hardware.jacobian( + "head", H_b, joints_val, original_length, original_density + ) ) assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) @@ -163,10 +159,10 @@ def test_jacobian_dot(): def test_fk(): - H_test = np.array(comp.forward_kinematics("l_sole", H_b, s_)) + H_test = np.array(comp.forward_kinematics("l_sole", H_b, joints_val)) H_with_hardware_test = np.array( comp_w_hardware.forward_kinematics( - "l_sole", H_b, s_, original_length, original_density + "l_sole", H_b, joints_val, original_length, original_density ) ) assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) @@ -174,10 +170,10 @@ def test_fk(): def test_fk_non_actuated(): - H_test = np.array(comp.forward_kinematics("head", H_b, s_)) + H_test = np.array(comp.forward_kinematics("head", H_b, joints_val)) H_with_hardware_test = np.array( comp_w_hardware.forward_kinematics( - "head", H_b, s_, original_length, original_density + "head", H_b, joints_val, original_length, original_density ) ) assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) @@ -185,28 +181,28 @@ def test_fk_non_actuated(): def test_bias_force(): - h_test = np.array(comp.bias_force(H_b, s_, vb_, s_dot_)) + h_test = np.array(comp.bias_force(H_b, joints_val, base_vel, joints_dot_val)) h_with_hardware_test = np.array( comp_w_hardware.bias_force( - H_b, s_, vb_, s_dot_, original_length, original_density + H_b, joints_val, base_vel, joints_dot_val, original_length, original_density ) ) assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4) def test_coriolis_term(): - C_test = np.array(comp.coriolis_term(H_b, s_, vb_, s_dot_)) + C_test = np.array(comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val)) C_with_hardware_test = np.array( comp_w_hardware.coriolis_term( - H_b, s_, vb_, s_dot_, original_length, original_density + H_b, joints_val, base_vel, joints_dot_val, original_length, original_density ) ) assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4) def test_gravity_term(): - G_test = comp.gravity_term(H_b, s_) + G_test = comp.gravity_term(H_b, joints_val) G_with_hardware_test = comp_w_hardware.gravity_term( - H_b, s_, original_length, original_density + H_b, joints_val, original_length, original_density ) assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4) From 90951038118a128126fe4304f32e787e7aae0b56 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 25 Jan 2024 18:23:24 +0100 Subject: [PATCH 17/20] Do not wrap array in a torch tensor if already a tensor --- src/adam/pytorch/torch_like.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/adam/pytorch/torch_like.py b/src/adam/pytorch/torch_like.py index 0bc98ea1..71962f65 100644 --- a/src/adam/pytorch/torch_like.py +++ b/src/adam/pytorch/torch_like.py @@ -59,6 +59,8 @@ def __matmul__(self, other: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": if type(self) is type(other): return TorchLike(self.array @ other.array) + if isinstance(other, torch.Tensor): + return TorchLike(self.array @ other) else: return TorchLike(self.array @ torch.tensor(other)) From 72857cbd005282f396a23cb69d68002904121293 Mon Sep 17 00:00:00 2001 From: Giuseppe L'Erario Date: Fri, 26 Jan 2024 14:02:33 +0100 Subject: [PATCH 18/20] Update src/adam/parametric/pytorch/computations_parametric.py Co-authored-by: Silvio Traversaro --- src/adam/parametric/pytorch/computations_parametric.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/adam/parametric/pytorch/computations_parametric.py b/src/adam/parametric/pytorch/computations_parametric.py index 08f9fb55..53bd107b 100644 --- a/src/adam/parametric/pytorch/computations_parametric.py +++ b/src/adam/parametric/pytorch/computations_parametric.py @@ -121,7 +121,7 @@ def forward_kinematics( self, frame, base_transform: torch.Tensor, - joint_postitions: torch.Tensor, + joint_positions: torch.Tensor, length_multiplier: torch.Tensor, densities: torch.Tensor, ) -> torch.Tensor: From 34256c75efeb3d1998e6b674b7edcb3c674128ea Mon Sep 17 00:00:00 2001 From: Giuseppe L'Erario Date: Fri, 26 Jan 2024 14:03:10 +0100 Subject: [PATCH 19/20] Update src/adam/parametric/pytorch/computations_parametric.py Co-authored-by: Silvio Traversaro --- src/adam/parametric/pytorch/computations_parametric.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/adam/parametric/pytorch/computations_parametric.py b/src/adam/parametric/pytorch/computations_parametric.py index 53bd107b..3a03584b 100644 --- a/src/adam/parametric/pytorch/computations_parametric.py +++ b/src/adam/parametric/pytorch/computations_parametric.py @@ -130,7 +130,7 @@ def forward_kinematics( Args: frame (str): The frame to which the fk will be computed base_transform (torch.tensor): The homogenous transform from base to world frame - joints_postition (torch.tensor): The joints position + joint_position (torch.tensor): The joints position length_multiplier (torch.tensor): The length multiplier of the parametrized links densities (torch.tensor): The densities of the parametrized links From be8934ba06bfa6dccdfd5782b87d489f20a26c6c Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 26 Jan 2024 14:30:37 +0100 Subject: [PATCH 20/20] Fix typo in variable name --- src/adam/parametric/pytorch/computations_parametric.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/adam/parametric/pytorch/computations_parametric.py b/src/adam/parametric/pytorch/computations_parametric.py index 3a03584b..531124f7 100644 --- a/src/adam/parametric/pytorch/computations_parametric.py +++ b/src/adam/parametric/pytorch/computations_parametric.py @@ -130,7 +130,7 @@ def forward_kinematics( Args: frame (str): The frame to which the fk will be computed base_transform (torch.tensor): The homogenous transform from base to world frame - joint_position (torch.tensor): The joints position + joint_positions (torch.tensor): The joints position length_multiplier (torch.tensor): The length multiplier of the parametrized links densities (torch.tensor): The densities of the parametrized links @@ -153,7 +153,7 @@ def forward_kinematics( self.rbdalgos.forward_kinematics( frame, base_transform, - joint_postitions, + joint_positions, ) ).array