From b231b38f03bf64210230654f43245c773eda3063 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 29 Feb 2024 18:00:20 +0100 Subject: [PATCH 01/32] Added PyCharm conf files to gitignore --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 2c558a5e..c73142d8 100644 --- a/.gitignore +++ b/.gitignore @@ -137,3 +137,6 @@ dmypy.json # Cython debug symbols cython_debug/ + +# Pycharm +.idea/* From 71f3732fe8c572d74b185f57fa4f75933b1c26d7 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 29 Feb 2024 18:01:32 +0100 Subject: [PATCH 02/32] Workaround issue while loading stickBot model --- .../parametric_factories/parametric_model.py | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/src/adam/parametric/model/parametric_factories/parametric_model.py b/src/adam/parametric/model/parametric_factories/parametric_model.py index 1a460944..cbcb3ed7 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_model.py +++ b/src/adam/parametric/model/parametric_factories/parametric_model.py @@ -2,9 +2,9 @@ from typing import List import urdf_parser_py.urdf - from adam.core.spatial_math import SpatialMath from adam.model import ModelFactory, StdJoint, StdLink, Link, Joint +from adam.model.std_factories.std_model import urdf_remove_sensors_tags from adam.parametric.model import ParmetricJoint, ParametricLink @@ -29,7 +29,22 @@ def __init__( if not path.exists(): raise FileExistsError(path) self.links_name_list = links_name_list - 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_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 self.length_multiplier = length_multiplier self.densities = densities From 48ae25b9697afe0d4639769d2b8ce69a3871e323 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 29 Feb 2024 18:02:39 +0100 Subject: [PATCH 03/32] Fixed order of arguments in get_total_mass and removed leftover forward_kinematics method In the parametric casadi implementation --- .../casadi/computations_parametric.py | 19 ++----------------- 1 file changed, 2 insertions(+), 17 deletions(-) diff --git a/src/adam/parametric/casadi/computations_parametric.py b/src/adam/parametric/casadi/computations_parametric.py index 7e1e8aaa..ef922a36 100644 --- a/src/adam/parametric/casadi/computations_parametric.py +++ b/src/adam/parametric/casadi/computations_parametric.py @@ -248,21 +248,7 @@ def gravity_term_fun(self) -> cs.Function: self.f_opts, ) - def forward_kinematics(self, frame, T_b, s) -> cs.Function: - """Computes the forward kinematics relative to the specified frame - - Args: - frame (str): The frame to which the fk will be computed - - Returns: - T_fk (casADi function): The fk represented as Homogenous transformation matrix - """ - - return self.rbdalgos.forward_kinematics( - frame, T_b, s, self.length_multiplier, self.densities - ) - - def get_total_mass(self) -> float: + def get_total_mass(self) -> cs.Function: """Returns the total mass of the robot Returns: @@ -270,6 +256,5 @@ def get_total_mass(self) -> float: """ m = self.rbdalgos.get_total_mass() return cs.Function( - "m", [self.densities, self.length_multiplier], [m], self.f_opts + "m", [self.length_multiplier, self.densities], [m], self.f_opts ) - return self.rbdalgos.get_total_mass() From d3c8b12fa5ee57f988d63a72e8d1ca5d96214a6e Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 5 Mar 2024 12:34:42 +0100 Subject: [PATCH 04/32] Fixed typos --- src/adam/parametric/model/__init__.py | 2 +- .../model/parametric_factories/parametric_joint.py | 4 ++-- .../model/parametric_factories/parametric_link.py | 10 +++++----- .../model/parametric_factories/parametric_model.py | 4 ++-- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/adam/parametric/model/__init__.py b/src/adam/parametric/model/__init__.py index 729aec3e..176c2349 100644 --- a/src/adam/parametric/model/__init__.py +++ b/src/adam/parametric/model/__init__.py @@ -2,6 +2,6 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -from .parametric_factories.parametric_joint import ParmetricJoint +from .parametric_factories.parametric_joint import ParametricJoint from .parametric_factories.parametric_link import ParametricLink from .parametric_factories.parametric_model import URDFParametricModelFactory diff --git a/src/adam/parametric/model/parametric_factories/parametric_joint.py b/src/adam/parametric/model/parametric_factories/parametric_joint.py index 302b755b..47366555 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_joint.py +++ b/src/adam/parametric/model/parametric_factories/parametric_joint.py @@ -8,7 +8,7 @@ from adam.parametric.model.parametric_factories.parametric_link import ParametricLink -class ParmetricJoint(Joint): +class ParametricJoint(Joint): """Parametric Joint class""" def __init__( @@ -43,7 +43,7 @@ def modify(self, parent_joint_offset: npt.ArrayLike): npt.ArrayLike: the origin of the joint, parametric with respect to the parent link dimensions """ - length = self.parent_parametric.get_principal_lenght_parametric() + length = self.parent_parametric.get_principal_length_parametric() # Ack for avoiding depending on casadi vo = self.parent_parametric.origin[2] xyz_rpy = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] diff --git a/src/adam/parametric/model/parametric_factories/parametric_link.py b/src/adam/parametric/model/parametric_factories/parametric_link.py index 389e417b..d5b5a891 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_link.py +++ b/src/adam/parametric/model/parametric_factories/parametric_link.py @@ -66,7 +66,7 @@ def __init__( self.inertial.inertia = self.I self.inertial.origin = self.origin - def get_principal_lenght(self): + def get_principal_length(self): """Method computing the principal link length, i.e. the dimension in which the kinematic chain grows""" xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] if self.geometry_type == Geometry.CYLINDER: @@ -86,7 +86,7 @@ def get_principal_lenght(self): raise Exception(f"THE GEOMETRY IS NOT SPECIFIED") return v_l - def get_principal_lenght_parametric(self): + def get_principal_length_parametric(self): """Method computing the principal link length parametric, i.e. the dimension in which the kinematic chain grows""" xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] if self.geometry_type == Geometry.CYLINDER: @@ -112,7 +112,7 @@ def compute_offset(self): npt.ArrayLike: link offset """ xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] - v_l = self.get_principal_lenght() + v_l = self.get_principal_length() v_o = xyz_rpy[2] if v_o < 0: link_offset = v_l / 2 + v_o @@ -127,7 +127,7 @@ def compute_joint_offset(self, joint_i, parent_offset): """ # Taking the principal direction i.e. the length xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] - v_l = self.get_principal_lenght() + v_l = self.get_principal_length() j_0 = joint_i.origin.xyz[2] v_o = xyz_rpy[2] if j_0 < 0: @@ -194,7 +194,7 @@ def modify_origin(self): origin = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] v_o = xyz_rpy[2] - length = self.get_principal_lenght_parametric() + length = self.get_principal_length_parametric() if v_o < 0: origin[2] = self.link_offset - length / 2 origin[0] = xyz_rpy[0] diff --git a/src/adam/parametric/model/parametric_factories/parametric_model.py b/src/adam/parametric/model/parametric_factories/parametric_model.py index cbcb3ed7..5c2a2092 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_model.py +++ b/src/adam/parametric/model/parametric_factories/parametric_model.py @@ -5,7 +5,7 @@ from adam.core.spatial_math import SpatialMath from adam.model import ModelFactory, StdJoint, StdLink, Link, Joint from adam.model.std_factories.std_model import urdf_remove_sensors_tags -from adam.parametric.model import ParmetricJoint, ParametricLink +from adam.parametric.model import ParametricJoint, ParametricLink class URDFParametricModelFactory(ModelFactory): @@ -89,7 +89,7 @@ def build_joint(self, joint: urdf_parser_py.urdf.Joint) -> Joint: self.length_multiplier[index_link], self.densities[index_link], ) - return ParmetricJoint(joint, self.math, parent_link_parametric) + return ParametricJoint(joint, self.math, parent_link_parametric) return StdJoint(joint, self.math) From d87d797100cde64b5741f6da778f1cb80e39ea58 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 5 Mar 2024 13:10:19 +0100 Subject: [PATCH 05/32] Updating the visual info after the parametric modifications --- .../parametric_factories/parametric_link.py | 32 ++++++++++++------- 1 file changed, 21 insertions(+), 11 deletions(-) diff --git a/src/adam/parametric/model/parametric_factories/parametric_link.py b/src/adam/parametric/model/parametric_factories/parametric_link.py index d5b5a891..dc63c7da 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_link.py +++ b/src/adam/parametric/model/parametric_factories/parametric_link.py @@ -1,6 +1,7 @@ import numpy.typing as npt import urdf_parser_py.urdf from enum import Enum +import copy from adam.core.spatial_math import SpatialMath from adam.model import Link @@ -53,8 +54,9 @@ def __init__( self.name = link.name self.length_multiplier = length_multiplier self.densities = densities - self.visuals = link.visual - self.geometry_type, self.visual_data = self.get_geometry(self.visuals) + self.original_visual = copy.deepcopy(link.visual) + self.visuals = [copy.deepcopy(link.visual)] + self.geometry_type, self.visual_data = self.get_geometry(self.original_visual) self.link = link self.link_offset = self.compute_offset() (self.volume, self.visual_data_new) = self.compute_volume() @@ -65,10 +67,11 @@ def __init__( self.inertial.mass = self.mass self.inertial.inertia = self.I self.inertial.origin = self.origin + self.update_visuals() def get_principal_length(self): """Method computing the principal link length, i.e. the dimension in which the kinematic chain grows""" - xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] + xyz_rpy = [*self.original_visual.origin.xyz, *self.original_visual.origin.rpy] if self.geometry_type == Geometry.CYLINDER: if xyz_rpy[3] < 0.0 or xyz_rpy[4] > 0.0: v_l = ( @@ -88,7 +91,7 @@ def get_principal_length(self): def get_principal_length_parametric(self): """Method computing the principal link length parametric, i.e. the dimension in which the kinematic chain grows""" - xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] + xyz_rpy = [*self.original_visual.origin.xyz, *self.original_visual.origin.rpy] if self.geometry_type == Geometry.CYLINDER: if xyz_rpy[3] < 0.0 or xyz_rpy[4] > 0.0: v_l = ( @@ -111,7 +114,7 @@ def compute_offset(self): Returns: npt.ArrayLike: link offset """ - xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] + xyz_rpy = [*self.original_visual.origin.xyz, *self.original_visual.origin.rpy] v_l = self.get_principal_length() v_o = xyz_rpy[2] if v_o < 0: @@ -126,10 +129,8 @@ def compute_joint_offset(self, joint_i, parent_offset): npt.ArrayLike: the child joint offset """ # Taking the principal direction i.e. the length - xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] v_l = self.get_principal_length() j_0 = joint_i.origin.xyz[2] - v_o = xyz_rpy[2] if j_0 < 0: joint_offset_temp = -(v_l + j_0 - parent_offset) joint_offset = joint_offset_temp @@ -170,7 +171,6 @@ def compute_volume(self): visual_data_new[1] = self.visual_data.radius # * self.length_multiplier[1] volume = math.pi * visual_data_new[1] ** 2 * visual_data_new[0] elif self.geometry_type == Geometry.SPHERE: - visual_data_new = 0.0 visual_data_new = self.visual_data.radius * self.length_multiplier volume = 4 * math.pi * visual_data_new**3 / 3 return volume, visual_data_new @@ -192,7 +192,7 @@ def modify_origin(self): (npt.ArrayLike): the link origin parametrized """ origin = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] + xyz_rpy = [*self.original_visual.origin.xyz, *self.original_visual.origin.rpy] v_o = xyz_rpy[2] length = self.get_principal_length_parametric() if v_o < 0: @@ -221,7 +221,7 @@ def compute_inertia_parametric(self): Formulas retrieved from https://en.wikipedia.org/wiki/List_of_moments_of_inertia """ I = I_parametric() - xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] + xyz_rpy = [*self.original_visual.origin.xyz, *self.original_visual.origin.rpy] if self.geometry_type == Geometry.BOX: I.ixx = ( self.mass @@ -281,7 +281,7 @@ def spatial_inertia(self) -> npt.ArrayLike: def homogeneous(self) -> npt.ArrayLike: """ Returns: - npt.ArrayLike: the homogeneus transform of the link + npt.ArrayLike: the homogeneous transform of the link """ o = self.math.factory.zeros(3) @@ -293,3 +293,13 @@ def homogeneous(self) -> npt.ArrayLike: o, rpy, ) + + def update_visuals(self): + if self.geometry_type == Geometry.BOX: + self.visuals[0].geometry.size = self.visual_data_new + self.visuals[0].origin.xyz[2] = self.origin[2] + elif self.geometry_type == Geometry.CYLINDER: + self.visuals[0].geometry.length = self.visual_data_new[0] + self.visuals[0].origin.xyz[2] = self.origin[2] + elif self.geometry_type == Geometry.SPHERE: + self.visuals[0].geometry.radius = self.visual_data_new From 430ac4db44e0377903827bb8a71602a58cc8ec10 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 5 Mar 2024 19:15:19 +0100 Subject: [PATCH 06/32] Initial draft to convert an ADAM model to an iDynTree model --- setup.cfg | 2 + src/adam/model/conversions/__init__.py | 0 src/adam/model/conversions/idyntree.py | 95 ++++++++++++++++++++++++++ 3 files changed, 97 insertions(+) create mode 100644 src/adam/model/conversions/__init__.py create mode 100644 src/adam/model/conversions/idyntree.py diff --git a/setup.cfg b/setup.cfg index b457f256..95d0d87a 100644 --- a/setup.cfg +++ b/setup.cfg @@ -54,6 +54,8 @@ test = icub-models black gitpython +conversions = + idyntree all = jax jaxlib diff --git a/src/adam/model/conversions/__init__.py b/src/adam/model/conversions/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/adam/model/conversions/idyntree.py b/src/adam/model/conversions/idyntree.py new file mode 100644 index 00000000..b0ad8e40 --- /dev/null +++ b/src/adam/model/conversions/idyntree.py @@ -0,0 +1,95 @@ +import idyntree.bindings +from idyntree.bindings import IJoint as idyn_joint +from idyntree.bindings import Link as idyn_link +from idyntree.bindings import Model as idyn_model +from idyntree.bindings import SolidShape as idyn_solid_shape +import numpy as np +import urdf_parser_py.urdf + + +from adam.model.model import Model +from adam.model.abc_factories import Link, Joint + + +def to_idyntree_solid_shape(visuals: urdf_parser_py.urdf.Visual) -> idyn_solid_shape: + """ + Convert an urdf visual to an iDynTree solid shape + :param visuals: The input visual + :return: The iDynTree solid shape + """ + if type(visuals.geometry) is urdf_parser_py.urdf.Box: + output = idyntree.bindings.Box() + output.setX(visuals.geometry.size[0]) + output.setY(visuals.geometry.size[1]) + output.setZ(visuals.geometry.size[2]) + return output + if type(visuals.geometry) is urdf_parser_py.urdf.Cylinder: + output = idyntree.bindings.Cylinder() + output.setRadius(visuals.geometry.radius) + output.setLength(visuals.geometry.length) + return output + + if type(visuals.geometry) is urdf_parser_py.urdf.Sphere: + output = idyntree.bindings.Sphere() + output.setRadius(visuals.geometry.radius) + return output + if type(visuals.geometry) is urdf_parser_py.urdf.Mesh: + output = idyntree.bindings.ExternalMesh() + output.setFilename(visuals.geometry.filename) + output.setScale(visuals.geometry.scale) + return output + + raise NotImplementedError("The visual type is not supported") + + +def to_idyntree_link(link: Link) -> [idyn_link, idyn_solid_shape]: + """ + Args: + link (Link): the link to convert + + Returns: + A tuple containing the iDynTree link and the iDynTree solid shape + """ + output = idyn_link() + I = link.inertial.inertia + inertia_matrix = np.array( + [[I.ixx, I.ixy, I.ixz], [I.ixy, I.iyy, I.iyz], [I.ixz, I.iyz, I.izz]] + ) + inertia_rotation = idyntree.bindings.Rotation.RPY( + link.inertial.origin.rpy[0], + link.inertial.origin.rpy[1], + link.inertial.origin.rpy[2], + ) + idyn_spatial_rotational_inertia = idyntree.bindings.RotationalInertia() + idyn_spatial_rotational_inertia.FromPython(inertia_matrix) + rotated_inertia = inertia_rotation * idyn_spatial_rotational_inertia + idyn_spatial_inertia = idyntree.bindings.SpatialInertia() + idyn_spatial_inertia.fromRotationalInertiaWrtCenterOfMass( + link.inertial.mass, link.inertial.origin, rotated_inertia + ) + output.setInertia(idyn_spatial_inertia) + + # Here I need to convert the visual to an idyntree solid shape + pass + + +def to_idyntree_joint(joint: Joint) -> idyn_joint: + """ + Args: + joint (Joint): the joint to convert + + Returns: + iDynTree.bindings.IJoint: the iDynTree joint + """ + pass + + +def to_idyntree_model(model: Model) -> idyn_model: + """ + Args: + model (Model): the model to convert + + Returns: + iDynTree.Model: the iDynTree model + """ + pass From bc5f5eeb37de0b832c00a3f0fb35ad993d8858fc Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 6 Mar 2024 11:43:43 +0100 Subject: [PATCH 07/32] Completed draft of conversion to iDynTree model To be tested --- src/adam/model/conversions/idyntree.py | 102 +++++++++++++++++++------ 1 file changed, 77 insertions(+), 25 deletions(-) diff --git a/src/adam/model/conversions/idyntree.py b/src/adam/model/conversions/idyntree.py index b0ad8e40..a66128a8 100644 --- a/src/adam/model/conversions/idyntree.py +++ b/src/adam/model/conversions/idyntree.py @@ -3,8 +3,10 @@ from idyntree.bindings import Link as idyn_link from idyntree.bindings import Model as idyn_model from idyntree.bindings import SolidShape as idyn_solid_shape +from idyntree.bindings import RotationalInertia as idyn_rotational_inertia import numpy as np import urdf_parser_py.urdf +from typing import List from adam.model.model import Model @@ -13,55 +15,58 @@ def to_idyntree_solid_shape(visuals: urdf_parser_py.urdf.Visual) -> idyn_solid_shape: """ - Convert an urdf visual to an iDynTree solid shape - :param visuals: The input visual - :return: The iDynTree solid shape + Args: + visuals (urdf_parser_py.urdf.Visual): the visual to convert + + Returns: + iDynTree.SolidShape: the iDynTree solid shape """ - if type(visuals.geometry) is urdf_parser_py.urdf.Box: + if isinstance(visuals.geometry, urdf_parser_py.urdf.Box): output = idyntree.bindings.Box() output.setX(visuals.geometry.size[0]) output.setY(visuals.geometry.size[1]) output.setZ(visuals.geometry.size[2]) return output - if type(visuals.geometry) is urdf_parser_py.urdf.Cylinder: + if isinstance(visuals.geometry, urdf_parser_py.urdf.Cylinder): output = idyntree.bindings.Cylinder() output.setRadius(visuals.geometry.radius) output.setLength(visuals.geometry.length) return output - if type(visuals.geometry) is urdf_parser_py.urdf.Sphere: + if isinstance(visuals.geometry, urdf_parser_py.urdf.Sphere): output = idyntree.bindings.Sphere() output.setRadius(visuals.geometry.radius) return output - if type(visuals.geometry) is urdf_parser_py.urdf.Mesh: + if isinstance(visuals.geometry, urdf_parser_py.urdf.Mesh): output = idyntree.bindings.ExternalMesh() output.setFilename(visuals.geometry.filename) output.setScale(visuals.geometry.scale) return output - raise NotImplementedError("The visual type is not supported") + raise NotImplementedError( + f"The visual type {visuals.geometry.__class__} is not supported" + ) -def to_idyntree_link(link: Link) -> [idyn_link, idyn_solid_shape]: +def to_idyntree_link(link: Link) -> [idyn_link, List[idyn_solid_shape]]: """ Args: link (Link): the link to convert Returns: - A tuple containing the iDynTree link and the iDynTree solid shape + A tuple containing the iDynTree link and the iDynTree solid shapes """ output = idyn_link() - I = link.inertial.inertia + input_inertia = link.inertial.inertia inertia_matrix = np.array( - [[I.ixx, I.ixy, I.ixz], [I.ixy, I.iyy, I.iyz], [I.ixz, I.iyz, I.izz]] + [ + [input_inertia.ixx, input_inertia.ixy, input_inertia.ixz], + [input_inertia.ixy, input_inertia.iyy, input_inertia.iyz], + [input_inertia.ixz, input_inertia.iyz, input_inertia.izz], + ] ) - inertia_rotation = idyntree.bindings.Rotation.RPY( - link.inertial.origin.rpy[0], - link.inertial.origin.rpy[1], - link.inertial.origin.rpy[2], - ) - idyn_spatial_rotational_inertia = idyntree.bindings.RotationalInertia() - idyn_spatial_rotational_inertia.FromPython(inertia_matrix) + inertia_rotation = idyntree.bindings.Rotation.RPY(*link.inertial.origin.rpy) + idyn_spatial_rotational_inertia = idyn_rotational_inertia.FromPython(inertia_matrix) rotated_inertia = inertia_rotation * idyn_spatial_rotational_inertia idyn_spatial_inertia = idyntree.bindings.SpatialInertia() idyn_spatial_inertia.fromRotationalInertiaWrtCenterOfMass( @@ -69,19 +74,45 @@ def to_idyntree_link(link: Link) -> [idyn_link, idyn_solid_shape]: ) output.setInertia(idyn_spatial_inertia) - # Here I need to convert the visual to an idyntree solid shape - pass + return output, [to_idyntree_solid_shape(v) for v in link.visuals] -def to_idyntree_joint(joint: Joint) -> idyn_joint: +def to_idyntree_joint(joint: Joint, parent_index: int, child_index: int) -> idyn_joint: """ Args: joint (Joint): the joint to convert - + parent_index (int): the parent link index + child_index (int): the child link index Returns: iDynTree.bindings.IJoint: the iDynTree joint """ - pass + + # TODO: consider limits + + rest_position = idyntree.bindings.Position.FromPython(joint.origin.xyz) # noqa + rest_rotation = idyntree.bindings.Rotation.RPY(*joint.origin.rpy) # noqa + rest_transform = idyntree.bindings.Transform(rest_rotation, rest_position) + + if joint.type == "fixed": + return idyntree.bindings.FixedJoint(parent_index, child_index, rest_transform) + + direction = idyntree.bindings.Direction.FromPython(joint.axis) + axis = idyntree.bindings.Axis(direction, idyntree.bindings.Position.Zero()) + + if joint.type in ["revolute", "continuous"]: + output = idyntree.bindings.RevoluteJoint() + output.setAttachedLinks(parent_index, child_index) + output.setRestTransform(rest_transform) + output.setAxis(axis, child_index, parent_index) + return output + if joint.type in ["prismatic"]: + output = idyntree.bindings.PrismaticJoint() + output.setAttachedLinks(parent_index, child_index) + output.setRestTransform(rest_transform) + output.setAxis(axis, child_index, parent_index) + return output + + NotImplementedError(f"The joint type {joint.type} is not supported") def to_idyntree_model(model: Model) -> idyn_model: @@ -92,4 +123,25 @@ def to_idyntree_model(model: Model) -> idyn_model: Returns: iDynTree.Model: the iDynTree model """ - pass + output = idyn_model() + output_visuals = [] + links_map = {} + for node in model.tree: + link, visuals = to_idyntree_link(node.link) + link_index = output.addLink(node.name, link) + assert output.isValidLinkIndex(link_index) + assert link_index == len(output_visuals) + output_visuals.append(visuals) + links_map[node.name] = link_index + + output.visualSolidShapes().resize(len(output_visuals)) + for i, visuals in enumerate(output_visuals): + output.visualSolidShapes().set(i, visuals) + + for node in model.tree: + for j in node.arcs: + joint = to_idyntree_joint(j, links_map[j.parent], links_map[j.child]) + joint_index = output.addJoint(j.name, joint) + assert output.isValidJointIndex(joint_index) + + return output From 515bc72707f068b854c8a6d374e62c6c246a5e36 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 6 Mar 2024 17:44:24 +0100 Subject: [PATCH 08/32] Added initial test for conversion to iDynTree --- src/adam/model/conversions/idyntree.py | 32 +++- tests/conversions/test_idyntree.py | 244 +++++++++++++++++++++++++ 2 files changed, 268 insertions(+), 8 deletions(-) create mode 100644 tests/conversions/test_idyntree.py diff --git a/src/adam/model/conversions/idyntree.py b/src/adam/model/conversions/idyntree.py index a66128a8..14035c98 100644 --- a/src/adam/model/conversions/idyntree.py +++ b/src/adam/model/conversions/idyntree.py @@ -1,3 +1,4 @@ +import copy import idyntree.bindings from idyntree.bindings import IJoint as idyn_joint from idyntree.bindings import Link as idyn_link @@ -66,11 +67,17 @@ def to_idyntree_link(link: Link) -> [idyn_link, List[idyn_solid_shape]]: ] ) inertia_rotation = idyntree.bindings.Rotation.RPY(*link.inertial.origin.rpy) - idyn_spatial_rotational_inertia = idyn_rotational_inertia.FromPython(inertia_matrix) + idyn_spatial_rotational_inertia = idyn_rotational_inertia() + for i in range(3): + for j in range(3): + idyn_spatial_rotational_inertia.setVal(i, j, inertia_matrix[i, j]) rotated_inertia = inertia_rotation * idyn_spatial_rotational_inertia idyn_spatial_inertia = idyntree.bindings.SpatialInertia() + com_position = idyntree.bindings.Position.FromPython(link.inertial.origin.xyz) idyn_spatial_inertia.fromRotationalInertiaWrtCenterOfMass( - link.inertial.mass, link.inertial.origin, rotated_inertia + link.inertial.mass, + com_position, + rotated_inertia, ) output.setInertia(idyn_spatial_inertia) @@ -96,8 +103,11 @@ def to_idyntree_joint(joint: Joint, parent_index: int, child_index: int) -> idyn if joint.type == "fixed": return idyntree.bindings.FixedJoint(parent_index, child_index, rest_transform) - direction = idyntree.bindings.Direction.FromPython(joint.axis) - axis = idyntree.bindings.Axis(direction, idyntree.bindings.Position.Zero()) + direction = idyntree.bindings.Direction(*joint.axis) + origin = idyntree.bindings.Position.Zero() + axis = idyntree.bindings.Axis() + axis.setDirection(direction) + axis.setOrigin(origin) if joint.type in ["revolute", "continuous"]: output = idyntree.bindings.RevoluteJoint() @@ -123,6 +133,9 @@ def to_idyntree_model(model: Model) -> idyn_model: Returns: iDynTree.Model: the iDynTree model """ + + # TODO: handle frames + output = idyn_model() output_visuals = [] links_map = {} @@ -134,9 +147,7 @@ def to_idyntree_model(model: Model) -> idyn_model: output_visuals.append(visuals) links_map[node.name] = link_index - output.visualSolidShapes().resize(len(output_visuals)) - for i, visuals in enumerate(output_visuals): - output.visualSolidShapes().set(i, visuals) + # TODO: handle visuals for node in model.tree: for j in node.arcs: @@ -144,4 +155,9 @@ def to_idyntree_model(model: Model) -> idyn_model: joint_index = output.addJoint(j.name, joint) assert output.isValidJointIndex(joint_index) - return output + model_reducer = idyntree.bindings.ModelLoader() + model_reducer.loadReducedModelFromFullModel(output, model.actuated_joints) + output_reduced = model_reducer.model().copy() + + assert output_reduced.isValid() + return output_reduced diff --git a/tests/conversions/test_idyntree.py b/tests/conversions/test_idyntree.py new file mode 100644 index 00000000..d323b600 --- /dev/null +++ b/tests/conversions/test_idyntree.py @@ -0,0 +1,244 @@ +# Copyright (C) 2024 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import logging + +import casadi as cs +import icub_models +import idyntree.swig as idyntree +import numpy as np +import pytest + +from adam.casadi import KinDynComputations +from adam.geometry import utils +from adam import Representations +from adam.model.conversions.idyntree import to_idyntree_model + +np.random.seed(42) + +model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) + +joints_name_list = [ + "torso_pitch", + "torso_roll", + "torso_yaw", + "l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "l_hip_pitch", + "l_hip_roll", + "l_hip_yaw", + "l_knee", + "l_ankle_pitch", + "l_ankle_roll", + "r_hip_pitch", + "r_hip_roll", + "r_hip_yaw", + "r_knee", + "r_ankle_pitch", + "r_ankle_roll", +] + + +def H_from_Pos_RPY_idyn(xyz, rpy): + T = idyntree.Transform.Identity() + R = idyntree.Rotation.RPY(rpy[0], rpy[1], rpy[2]) + p = idyntree.Position() + [p.setVal(i, xyz[i]) for i in range(3)] + T.setRotation(R) + T.setPosition(p) + return T + + +logging.basicConfig(level=logging.DEBUG) +logging.debug("Showing the robot tree.") + +root_link = "root_link" +comp = KinDynComputations(model_path, joints_name_list, root_link) +comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) + +kinDyn = idyntree.KinDynComputations() +kinDyn.loadRobotModel(to_idyntree_model(comp.rbdalgos.model)) +kinDyn.setFloatingBase(root_link) +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 +# joints quantitites +joints_val = (np.random.rand(n_dofs) - 0.5) * 5 +joints_dot_val = (np.random.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) + + +def test_mass_matrix(): + M = comp.mass_matrix_fun() + mass_mx = idyntree.MatrixDynSize() + kinDyn.getFreeFloatingMassMatrix(mass_mx) + mass_mxNumpy = mass_mx.toNumPy() + mass_test = cs.DM(M(H_b, joints_val)) + mass_test2 = cs.DM(comp.mass_matrix(H_b, joints_val)) + assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) + assert mass_test2 - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(): + Jcm = comp.centroidal_momentum_matrix_fun() + cmm_idyntree = idyntree.MatrixDynSize() + kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) + cmm_idyntreeNumpy = cmm_idyntree.toNumPy() + Jcm_test = cs.DM(Jcm(H_b, joints_val)) + Jcm_test2 = cs.DM(comp.centroidal_momentum_matrix(H_b, joints_val)) + assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) + assert Jcm_test2 - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(): + com_f = comp.CoM_position_fun() + CoM_test = cs.DM(com_f(H_b, joints_val)) + CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() + CoM_test2 = cs.DM(comp.CoM_position(H_b, joints_val)) + assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) + assert CoM_test2 - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) + + +# def test_total_mass(): +# assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx( +# 0.0, abs=1e-5 +# ) + + +# def test_jacobian(): +# J_tot = comp.jacobian_fun("l_sole") +# iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) +# kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) +# iDynNumpyJ_ = iDyntreeJ_.toNumPy() +# J_test = cs.DM(J_tot(H_b, joints_val)) +# J_test2 = cs.DM(comp.jacobian("l_sole", H_b, joints_val)) +# assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) +# assert iDynNumpyJ_ - J_test2 == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(): + J_tot = comp.jacobian_fun("head") + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_test = cs.DM(J_tot(H_b, joints_val)) + J_test2 = cs.DM(comp.jacobian("head", H_b, joints_val)) + assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) + assert iDynNumpyJ_ - J_test2 == pytest.approx(0.0, abs=1e-5) + + +# def test_jacobian_dot(): +# J_dot = comp.jacobian_dot_fun("l_sole") +# Jdotnu = kinDyn.getFrameBiasAcc("l_sole") +# Jdot_nu = Jdotnu.toNumPy() +# J_dot_nu_test = J_dot(H_b, joints_val, base_vel, joints_dot_val) @ np.concatenate( +# (base_vel, joints_dot_val) +# ) +# J_dot_nu_test2 = cs.DM( +# comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) +# ) @ np.concatenate((base_vel, joints_dot_val)) +# assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) +# assert Jdot_nu - J_dot_nu_test2 == pytest.approx(0.0, abs=1e-5) + + +# def test_fk(): +# H_idyntree = kinDyn.getWorldTransform("l_sole") +# p_idy2np = H_idyntree.getPosition().toNumPy() +# R_idy2np = H_idyntree.getRotation().toNumPy() +# T = comp.forward_kinematics_fun("l_sole") +# H_test = cs.DM(T(H_b, joints_val)) +# H_test2 = cs.DM(comp.forward_kinematics("l_sole", H_b, joints_val)) +# assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) +# assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) +# assert R_idy2np - H_test2[:3, :3] == pytest.approx(0.0, abs=1e-5) +# assert p_idy2np - H_test2[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(): + H_idyntree = kinDyn.getWorldTransform("head") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + T = comp.forward_kinematics_fun("head") + H_test = cs.DM(T(H_b, joints_val)) + H_test2 = cs.DM(comp.forward_kinematics("head", H_b, joints_val)) + assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + assert R_idy2np - H_test2[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert p_idy2np - H_test2[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(): + h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(h_iDyn) + h_iDyn_np = np.concatenate( + (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) + ) + h = comp.bias_force_fun() + h_test = cs.DM(h(H_b, joints_val, base_vel, joints_dot_val)) + h_test2 = cs.DM(comp.bias_force(H_b, joints_val, base_vel, joints_dot_val)) + assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) + assert h_iDyn_np - h_test2 == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + g0 = idyntree.Vector3() + g0.zero() + kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) + C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(C_iDyn) + C_iDyn_np = np.concatenate( + (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) + ) + C = comp.coriolis_term_fun() + C_test = cs.DM(C(H_b, joints_val, base_vel, joints_dot_val)) + C_test2 = cs.DM(comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val)) + assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) + assert C_iDyn_np - C_test2 == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(): + base_vel_0 = np.zeros(6) + joints_dot_val_0 = np.zeros(n_dofs) + kinDyn.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) + kinDyn.setRobotState(H_b, joints_val, base_vel_0, joints_dot_val_0, g) + G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(G_iDyn) + G_iDyn_np = np.concatenate( + (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) + ) + G = comp.gravity_term_fun() + G_test = cs.DM(G(H_b, joints_val)) + G_test2 = cs.DM(comp.gravity_term(H_b, joints_val)) + assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) + assert G_iDyn_np - G_test2 == pytest.approx(0.0, abs=1e-4) + + +# def test_relative_jacobian(): +# eye = np.eye(4) +# kinDyn.setRobotState(eye, joints_val, base_vel, joints_dot_val, g) +# iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) +# kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) +# iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] +# J_fun = comp.relative_jacobian_fun("l_sole") +# J_test = cs.DM(J_fun(joints_val)) +# J_test2 = cs.DM(comp.relative_jacobian("l_sole", joints_val)) +# assert iDynNumpyRelativeJ - J_test == pytest.approx(0.0, abs=1e-4) +# assert iDynNumpyRelativeJ - J_test2 == pytest.approx(0.0, abs=1e-4) From 7a92d12914b6ddd5ce6fb3993fd12c35b89ee777 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 7 Mar 2024 14:20:28 +0100 Subject: [PATCH 09/32] Set visuals --- src/adam/model/conversions/idyntree.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/adam/model/conversions/idyntree.py b/src/adam/model/conversions/idyntree.py index 14035c98..a1657e6a 100644 --- a/src/adam/model/conversions/idyntree.py +++ b/src/adam/model/conversions/idyntree.py @@ -139,6 +139,7 @@ def to_idyntree_model(model: Model) -> idyn_model: output = idyn_model() output_visuals = [] links_map = {} + for node in model.tree: link, visuals = to_idyntree_link(node.link) link_index = output.addLink(node.name, link) @@ -146,8 +147,14 @@ def to_idyntree_model(model: Model) -> idyn_model: assert link_index == len(output_visuals) output_visuals.append(visuals) links_map[node.name] = link_index - - # TODO: handle visuals + for child in node.children: + if any([child.name == frame for frame in model.frames]): + print("Frame found: ", child) + + for i, visuals in enumerate(output_visuals): + output.visualSolidShapes().clearSingleLinkSolidShapes(i) + for visual in visuals: + output.visualSolidShapes().addSingleLinkSolidShape(i, visual) for node in model.tree: for j in node.arcs: From dc73658a8a239c5b10b79d8b3a95a9c585fd6e73 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 7 Mar 2024 16:46:07 +0100 Subject: [PATCH 10/32] Added handling of frames in the conversion to iDynTree --- src/adam/model/conversions/idyntree.py | 31 ++++++-- tests/conversions/test_idyntree.py | 98 +++++++++++++------------- 2 files changed, 74 insertions(+), 55 deletions(-) diff --git a/src/adam/model/conversions/idyntree.py b/src/adam/model/conversions/idyntree.py index a1657e6a..321e8caa 100644 --- a/src/adam/model/conversions/idyntree.py +++ b/src/adam/model/conversions/idyntree.py @@ -98,7 +98,9 @@ def to_idyntree_joint(joint: Joint, parent_index: int, child_index: int) -> idyn rest_position = idyntree.bindings.Position.FromPython(joint.origin.xyz) # noqa rest_rotation = idyntree.bindings.Rotation.RPY(*joint.origin.rpy) # noqa - rest_transform = idyntree.bindings.Transform(rest_rotation, rest_position) + rest_transform = idyntree.bindings.Transform() + rest_transform.setRotation(rest_rotation) + rest_transform.setPosition(rest_position) if joint.type == "fixed": return idyntree.bindings.FixedJoint(parent_index, child_index, rest_transform) @@ -134,8 +136,6 @@ def to_idyntree_model(model: Model) -> idyn_model: iDynTree.Model: the iDynTree model """ - # TODO: handle frames - output = idyn_model() output_visuals = [] links_map = {} @@ -147,9 +147,6 @@ def to_idyntree_model(model: Model) -> idyn_model: assert link_index == len(output_visuals) output_visuals.append(visuals) links_map[node.name] = link_index - for child in node.children: - if any([child.name == frame for frame in model.frames]): - print("Frame found: ", child) for i, visuals in enumerate(output_visuals): output.visualSolidShapes().clearSingleLinkSolidShapes(i) @@ -158,10 +155,32 @@ def to_idyntree_model(model: Model) -> idyn_model: for node in model.tree: for j in node.arcs: + assert j.name not in model.frames joint = to_idyntree_joint(j, links_map[j.parent], links_map[j.child]) joint_index = output.addJoint(j.name, joint) assert output.isValidJointIndex(joint_index) + frames_list = [f + "_fixed_joint" for f in model.frames] # noqa + for name in model.joints: + if name in frames_list: + joint = model.joints[name] # noqa + frame_position = idyntree.bindings.Position.FromPython( + joint.origin.xyz # noqa + ) + frame_transform = idyntree.bindings.Transform() + frame_transform.setRotation( + idyntree.bindings.Rotation.RPY(*joint.origin.rpy) + ) + frame_transform.setPosition(frame_position) + frame_name = joint.name.replace("_fixed_joint", "") + + ok = output.addAdditionalFrameToLink( + joint.parent, + frame_name, + frame_transform, + ) + assert ok + model_reducer = idyntree.bindings.ModelLoader() model_reducer.loadReducedModelFromFullModel(output, model.actuated_joints) output_reduced = model_reducer.model().copy() diff --git a/tests/conversions/test_idyntree.py b/tests/conversions/test_idyntree.py index d323b600..6f26a1cc 100644 --- a/tests/conversions/test_idyntree.py +++ b/tests/conversions/test_idyntree.py @@ -117,21 +117,21 @@ def test_CoM_pos(): assert CoM_test2 - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) -# def test_total_mass(): -# assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx( -# 0.0, abs=1e-5 -# ) +def test_total_mass(): + assert comp.get_total_mass() - kinDyn.model().getTotalMass() == pytest.approx( + 0.0, abs=1e-5 + ) -# def test_jacobian(): -# J_tot = comp.jacobian_fun("l_sole") -# iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) -# kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) -# iDynNumpyJ_ = iDyntreeJ_.toNumPy() -# J_test = cs.DM(J_tot(H_b, joints_val)) -# J_test2 = cs.DM(comp.jacobian("l_sole", H_b, joints_val)) -# assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) -# assert iDynNumpyJ_ - J_test2 == pytest.approx(0.0, abs=1e-5) +def test_jacobian(): + J_tot = comp.jacobian_fun("l_sole") + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_test = cs.DM(J_tot(H_b, joints_val)) + J_test2 = cs.DM(comp.jacobian("l_sole", H_b, joints_val)) + assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) + assert iDynNumpyJ_ - J_test2 == pytest.approx(0.0, abs=1e-5) def test_jacobian_non_actuated(): @@ -145,31 +145,31 @@ def test_jacobian_non_actuated(): assert iDynNumpyJ_ - J_test2 == pytest.approx(0.0, abs=1e-5) -# def test_jacobian_dot(): -# J_dot = comp.jacobian_dot_fun("l_sole") -# Jdotnu = kinDyn.getFrameBiasAcc("l_sole") -# Jdot_nu = Jdotnu.toNumPy() -# J_dot_nu_test = J_dot(H_b, joints_val, base_vel, joints_dot_val) @ np.concatenate( -# (base_vel, joints_dot_val) -# ) -# J_dot_nu_test2 = cs.DM( -# comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) -# ) @ np.concatenate((base_vel, joints_dot_val)) -# assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) -# assert Jdot_nu - J_dot_nu_test2 == pytest.approx(0.0, abs=1e-5) - - -# def test_fk(): -# H_idyntree = kinDyn.getWorldTransform("l_sole") -# p_idy2np = H_idyntree.getPosition().toNumPy() -# R_idy2np = H_idyntree.getRotation().toNumPy() -# T = comp.forward_kinematics_fun("l_sole") -# H_test = cs.DM(T(H_b, joints_val)) -# H_test2 = cs.DM(comp.forward_kinematics("l_sole", H_b, joints_val)) -# assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) -# assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) -# assert R_idy2np - H_test2[:3, :3] == pytest.approx(0.0, abs=1e-5) -# assert p_idy2np - H_test2[:3, 3] == pytest.approx(0.0, abs=1e-5) +def test_jacobian_dot(): + J_dot = comp.jacobian_dot_fun("l_sole") + Jdotnu = kinDyn.getFrameBiasAcc("l_sole") + Jdot_nu = Jdotnu.toNumPy() + J_dot_nu_test = J_dot(H_b, joints_val, base_vel, joints_dot_val) @ np.concatenate( + (base_vel, joints_dot_val) + ) + J_dot_nu_test2 = cs.DM( + comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) + ) @ np.concatenate((base_vel, joints_dot_val)) + assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) + assert Jdot_nu - J_dot_nu_test2 == pytest.approx(0.0, abs=1e-5) + + +def test_fk(): + H_idyntree = kinDyn.getWorldTransform("l_sole") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + T = comp.forward_kinematics_fun("l_sole") + H_test = cs.DM(T(H_b, joints_val)) + H_test2 = cs.DM(comp.forward_kinematics("l_sole", H_b, joints_val)) + assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + assert R_idy2np - H_test2[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert p_idy2np - H_test2[:3, 3] == pytest.approx(0.0, abs=1e-5) def test_fk_non_actuated(): @@ -231,14 +231,14 @@ def test_gravity_term(): assert G_iDyn_np - G_test2 == pytest.approx(0.0, abs=1e-4) -# def test_relative_jacobian(): -# eye = np.eye(4) -# kinDyn.setRobotState(eye, joints_val, base_vel, joints_dot_val, g) -# iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) -# kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) -# iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] -# J_fun = comp.relative_jacobian_fun("l_sole") -# J_test = cs.DM(J_fun(joints_val)) -# J_test2 = cs.DM(comp.relative_jacobian("l_sole", joints_val)) -# assert iDynNumpyRelativeJ - J_test == pytest.approx(0.0, abs=1e-4) -# assert iDynNumpyRelativeJ - J_test2 == pytest.approx(0.0, abs=1e-4) +def test_relative_jacobian(): + eye = np.eye(4) + kinDyn.setRobotState(eye, joints_val, base_vel, joints_dot_val, g) + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) + iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] + J_fun = comp.relative_jacobian_fun("l_sole") + J_test = cs.DM(J_fun(joints_val)) + J_test2 = cs.DM(comp.relative_jacobian("l_sole", joints_val)) + assert iDynNumpyRelativeJ - J_test == pytest.approx(0.0, abs=1e-4) + assert iDynNumpyRelativeJ - J_test2 == pytest.approx(0.0, abs=1e-4) From 9ddb136af05355494034cbf692f8bdcc8320cd04 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 7 Mar 2024 16:53:36 +0100 Subject: [PATCH 11/32] Removed useless aliases in iDynTree conversion --- src/adam/model/conversions/idyntree.py | 30 +++++++++++++------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/adam/model/conversions/idyntree.py b/src/adam/model/conversions/idyntree.py index 321e8caa..10bf58b7 100644 --- a/src/adam/model/conversions/idyntree.py +++ b/src/adam/model/conversions/idyntree.py @@ -1,10 +1,4 @@ -import copy import idyntree.bindings -from idyntree.bindings import IJoint as idyn_joint -from idyntree.bindings import Link as idyn_link -from idyntree.bindings import Model as idyn_model -from idyntree.bindings import SolidShape as idyn_solid_shape -from idyntree.bindings import RotationalInertia as idyn_rotational_inertia import numpy as np import urdf_parser_py.urdf from typing import List @@ -14,7 +8,9 @@ from adam.model.abc_factories import Link, Joint -def to_idyntree_solid_shape(visuals: urdf_parser_py.urdf.Visual) -> idyn_solid_shape: +def to_idyntree_solid_shape( + visuals: urdf_parser_py.urdf.Visual, +) -> idyntree.bindings.SolidShape: """ Args: visuals (urdf_parser_py.urdf.Visual): the visual to convert @@ -49,7 +45,9 @@ def to_idyntree_solid_shape(visuals: urdf_parser_py.urdf.Visual) -> idyn_solid_s ) -def to_idyntree_link(link: Link) -> [idyn_link, List[idyn_solid_shape]]: +def to_idyntree_link( + link: Link, +) -> [idyntree.bindings.Link, List[idyntree.bindings.SolidShape]]: """ Args: link (Link): the link to convert @@ -57,7 +55,7 @@ def to_idyntree_link(link: Link) -> [idyn_link, List[idyn_solid_shape]]: Returns: A tuple containing the iDynTree link and the iDynTree solid shapes """ - output = idyn_link() + output = idyntree.bindings.Link() input_inertia = link.inertial.inertia inertia_matrix = np.array( [ @@ -67,7 +65,7 @@ def to_idyntree_link(link: Link) -> [idyn_link, List[idyn_solid_shape]]: ] ) inertia_rotation = idyntree.bindings.Rotation.RPY(*link.inertial.origin.rpy) - idyn_spatial_rotational_inertia = idyn_rotational_inertia() + idyn_spatial_rotational_inertia = idyntree.bindings.RotationalInertia() for i in range(3): for j in range(3): idyn_spatial_rotational_inertia.setVal(i, j, inertia_matrix[i, j]) @@ -84,7 +82,9 @@ def to_idyntree_link(link: Link) -> [idyn_link, List[idyn_solid_shape]]: return output, [to_idyntree_solid_shape(v) for v in link.visuals] -def to_idyntree_joint(joint: Joint, parent_index: int, child_index: int) -> idyn_joint: +def to_idyntree_joint( + joint: Joint, parent_index: int, child_index: int +) -> idyntree.bindings.IJoint: """ Args: joint (Joint): the joint to convert @@ -127,7 +127,7 @@ def to_idyntree_joint(joint: Joint, parent_index: int, child_index: int) -> idyn NotImplementedError(f"The joint type {joint.type} is not supported") -def to_idyntree_model(model: Model) -> idyn_model: +def to_idyntree_model(model: Model) -> idyntree.bindings.Model: """ Args: model (Model): the model to convert @@ -136,7 +136,7 @@ def to_idyntree_model(model: Model) -> idyn_model: iDynTree.Model: the iDynTree model """ - output = idyn_model() + output = idyntree.bindings.Model() output_visuals = [] links_map = {} @@ -160,10 +160,10 @@ def to_idyntree_model(model: Model) -> idyn_model: joint_index = output.addJoint(j.name, joint) assert output.isValidJointIndex(joint_index) - frames_list = [f + "_fixed_joint" for f in model.frames] # noqa + frames_list = [f + "_fixed_joint" for f in model.frames] # noqa for name in model.joints: if name in frames_list: - joint = model.joints[name] # noqa + joint = model.joints[name] # noqa frame_position = idyntree.bindings.Position.FromPython( joint.origin.xyz # noqa ) From 72e7cb62aacda43c5649bc2314d832e0f5b08ec1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 7 Mar 2024 18:32:02 +0100 Subject: [PATCH 12/32] Adding check when setting visuals to avoid CI failures --- src/adam/model/conversions/idyntree.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/adam/model/conversions/idyntree.py b/src/adam/model/conversions/idyntree.py index 10bf58b7..94cb711c 100644 --- a/src/adam/model/conversions/idyntree.py +++ b/src/adam/model/conversions/idyntree.py @@ -149,6 +149,9 @@ def to_idyntree_model(model: Model) -> idyntree.bindings.Model: links_map[node.name] = link_index for i, visuals in enumerate(output_visuals): + # To be removed after the change is in iDynTree + if not hasattr(output.visualSolidShapes(), "clearSingleLinkSolidShapes"): + continue output.visualSolidShapes().clearSingleLinkSolidShapes(i) for visual in visuals: output.visualSolidShapes().addSingleLinkSolidShape(i, visual) From af2d1c46751d01aebcf8a7a37598d7960e3a79e4 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 7 Mar 2024 18:37:30 +0100 Subject: [PATCH 13/32] Removed useless origin field in Link and added Pose for type hinting --- src/adam/model/abc_factories.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/adam/model/abc_factories.py b/src/adam/model/abc_factories.py index 9e406026..5f2c6fc1 100644 --- a/src/adam/model/abc_factories.py +++ b/src/adam/model/abc_factories.py @@ -7,6 +7,14 @@ from adam.core.spatial_math import SpatialMath +@dataclasses.dataclass +class Pose: + """Pose class""" + + xyz: List + rpy: List + + @dataclasses.dataclass class Joint(abc.ABC): """Base Joint class. You need to fill at least these fields""" @@ -17,7 +25,7 @@ class Joint(abc.ABC): child: str type: str axis: List - origin: List + origin: Pose limit: List idx: int """ @@ -71,7 +79,6 @@ class Link(abc.ABC): visuals: List inertial: Inertial collisions: List - origin: List @abc.abstractmethod def spatial_inertia(self) -> npt.ArrayLike: From 603d99741b19e4525365f4cd92b7eb1b4c3d603c Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 7 Mar 2024 18:45:34 +0100 Subject: [PATCH 14/32] Removed usages of link origin. Using link.inertial.origin instead --- src/adam/model/std_factories/std_link.py | 1 - .../parametric_factories/parametric_joint.py | 2 +- .../parametric_factories/parametric_link.py | 23 +++++++++---------- 3 files changed, 12 insertions(+), 14 deletions(-) diff --git a/src/adam/model/std_factories/std_link.py b/src/adam/model/std_factories/std_link.py index cefac67c..108e7bb6 100644 --- a/src/adam/model/std_factories/std_link.py +++ b/src/adam/model/std_factories/std_link.py @@ -14,7 +14,6 @@ def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath): self.visuals = link.visuals self.inertial = link.inertial self.collisions = link.collisions - self.origin = link.origin # if the link has inertial properties, but the origin is None, let's add it if link.inertial is not None and link.inertial.origin is None: diff --git a/src/adam/parametric/model/parametric_factories/parametric_joint.py b/src/adam/parametric/model/parametric_factories/parametric_joint.py index 47366555..05b18618 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_joint.py +++ b/src/adam/parametric/model/parametric_factories/parametric_joint.py @@ -45,7 +45,7 @@ def modify(self, parent_joint_offset: npt.ArrayLike): length = self.parent_parametric.get_principal_length_parametric() # Ack for avoiding depending on casadi - vo = self.parent_parametric.origin[2] + vo = self.parent_parametric.inertial.origin[2] xyz_rpy = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] xyz_rpy[0] = self.joint.origin.xyz[0] xyz_rpy[1] = self.joint.origin.xyz[1] diff --git a/src/adam/parametric/model/parametric_factories/parametric_link.py b/src/adam/parametric/model/parametric_factories/parametric_link.py index dc63c7da..48630145 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_link.py +++ b/src/adam/parametric/model/parametric_factories/parametric_link.py @@ -62,11 +62,10 @@ def __init__( (self.volume, self.visual_data_new) = self.compute_volume() self.mass = self.compute_mass() self.I = self.compute_inertia_parametric() - self.origin = self.modify_origin() self.inertial = Inertial(self.mass) self.inertial.mass = self.mass self.inertial.inertia = self.I - self.inertial.origin = self.origin + self.inertial.origin = self.modify_origin() self.update_visuals() def get_principal_length(self): @@ -272,10 +271,10 @@ def spatial_inertia(self) -> npt.ArrayLike: I = self.I mass = self.mass o = self.math.factory.zeros(3) - o[0] = self.origin[0] - o[1] = self.origin[1] - o[2] = self.origin[2] - rpy = self.origin[3:] + o[0] = self.inertial.origin[0] + o[1] = self.inertial.origin[1] + o[2] = self.inertial.origin[2] + rpy = self.inertial.origin[3:] return self.math.spatial_inertial_with_parameters(I, mass, o, rpy) def homogeneous(self) -> npt.ArrayLike: @@ -285,10 +284,10 @@ def homogeneous(self) -> npt.ArrayLike: """ o = self.math.factory.zeros(3) - o[0] = self.origin[0] - o[1] = self.origin[1] - o[2] = self.origin[2] - rpy = self.origin[3:] + o[0] = self.inertial.origin[0] + o[1] = self.inertial.origin[1] + o[2] = self.inertial.origin[2] + rpy = self.inertial.origin[3:] return self.math.H_from_Pos_RPY( o, rpy, @@ -297,9 +296,9 @@ def homogeneous(self) -> npt.ArrayLike: def update_visuals(self): if self.geometry_type == Geometry.BOX: self.visuals[0].geometry.size = self.visual_data_new - self.visuals[0].origin.xyz[2] = self.origin[2] + self.visuals[0].origin.xyz[2] = self.inertial.origin[2] elif self.geometry_type == Geometry.CYLINDER: self.visuals[0].geometry.length = self.visual_data_new[0] - self.visuals[0].origin.xyz[2] = self.origin[2] + self.visuals[0].origin.xyz[2] = self.inertial.origin[2] elif self.geometry_type == Geometry.SPHERE: self.visuals[0].geometry.radius = self.visual_data_new From 8638677ffeefe15f543c6bf619ec27c1fd9ae6d3 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 7 Mar 2024 19:03:29 +0100 Subject: [PATCH 15/32] Added Inertia type hint and fixed some warnings --- src/adam/model/abc_factories.py | 28 ++++++++++++++++-------- src/adam/model/std_factories/std_link.py | 8 +++---- 2 files changed, 22 insertions(+), 14 deletions(-) diff --git a/src/adam/model/abc_factories.py b/src/adam/model/abc_factories.py index 5f2c6fc1..fa144e25 100644 --- a/src/adam/model/abc_factories.py +++ b/src/adam/model/abc_factories.py @@ -11,8 +11,20 @@ class Pose: """Pose class""" - xyz: List - rpy: List + xyz: npt.ArrayLike + rpy: npt.ArrayLike + + +@dataclasses.dataclass +class Inertia: + """Inertia class""" + + ixx: npt.ArrayLike + ixy: npt.ArrayLike + ixz: npt.ArrayLike + iyy: npt.ArrayLike + iyz: npt.ArrayLike + izz: npt.ArrayLike @dataclasses.dataclass @@ -66,8 +78,8 @@ class Inertial: """Inertial description""" mass: npt.ArrayLike - inertia = npt.ArrayLike - origin = npt.ArrayLike + inertia = Inertia + origin = Pose @dataclasses.dataclass @@ -83,11 +95,9 @@ class Link(abc.ABC): @abc.abstractmethod def spatial_inertia(self) -> npt.ArrayLike: """ - Args: - link (Link): Link - Returns: - npt.ArrayLike: the 6x6 inertia matrix expressed at the origin of the link (with rotation) + npt.ArrayLike: the 6x6 inertia matrix expressed at + the origin of the link (with rotation) """ pass @@ -95,7 +105,7 @@ def spatial_inertia(self) -> npt.ArrayLike: def homogeneous(self) -> npt.ArrayLike: """ Returns: - npt.ArrayLike: the homogeneus transform of the link + npt.ArrayLike: the homogeneous transform of the link """ pass diff --git a/src/adam/model/std_factories/std_link.py b/src/adam/model/std_factories/std_link.py index 108e7bb6..9d90829d 100644 --- a/src/adam/model/std_factories/std_link.py +++ b/src/adam/model/std_factories/std_link.py @@ -22,11 +22,9 @@ def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath): def spatial_inertia(self) -> npt.ArrayLike: """ - Args: - link (Link): Link - Returns: - npt.ArrayLike: the 6x6 inertia matrix expressed at the origin of the link (with rotation) + npt.ArrayLike: the 6x6 inertia matrix expressed at + the origin of the link (with rotation) """ I = self.inertial.inertia mass = self.inertial.mass @@ -37,7 +35,7 @@ def spatial_inertia(self) -> npt.ArrayLike: def homogeneous(self) -> npt.ArrayLike: """ Returns: - npt.ArrayLike: the homogeneus transform of the link + npt.ArrayLike: the homogeneous transform of the link """ return self.math.H_from_Pos_RPY( self.inertial.origin.xyz, From 55e857db467add1606258f843ba296f3706e4c60 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 12:37:29 +0100 Subject: [PATCH 16/32] Using List in Pose type hint to avoid warnings about presence of [] operator --- src/adam/model/abc_factories.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/adam/model/abc_factories.py b/src/adam/model/abc_factories.py index fa144e25..ade55f67 100644 --- a/src/adam/model/abc_factories.py +++ b/src/adam/model/abc_factories.py @@ -11,8 +11,8 @@ class Pose: """Pose class""" - xyz: npt.ArrayLike - rpy: npt.ArrayLike + xyz: List + rpy: List @dataclasses.dataclass From df574644bfdd7792fd68cbec469d5656d3d1395f Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 12:39:02 +0100 Subject: [PATCH 17/32] Avoiding to use iyx in the inertia. It should be symmetric anyhow --- src/adam/core/spatial_math.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py index 0fc86c32..4200c142 100644 --- a/src/adam/core/spatial_math.py +++ b/src/adam/core/spatial_math.py @@ -417,7 +417,7 @@ def spatial_inertial_with_parameters(self, I, mass, c, rpy): R_temp = self.R_from_RPY(rpy) inertia_matrix = self.vertcat( self.horzcat(I.ixx, I.ixy, I.ixz), - self.horzcat(I.iyx, I.iyy, I.iyz), + self.horzcat(I.ixy, I.iyy, I.iyz), self.horzcat(I.ixz, I.iyz, I.izz), ) From a5e21bb284ff0e0a5937efc155b29d7f35faa6e9 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 12:49:09 +0100 Subject: [PATCH 18/32] Added missing self in method definitions of SpatialMath --- src/adam/core/spatial_math.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py index 4200c142..13f4e08e 100644 --- a/src/adam/core/spatial_math.py +++ b/src/adam/core/spatial_math.py @@ -107,7 +107,7 @@ def factory(self) -> ArrayLikeFactory: return self._factory @abc.abstractmethod - def vertcat(x: npt.ArrayLike) -> npt.ArrayLike: + def vertcat(self, x: npt.ArrayLike) -> npt.ArrayLike: """ Args: x (npt.ArrayLike): elements @@ -118,7 +118,7 @@ def vertcat(x: npt.ArrayLike) -> npt.ArrayLike: pass @abc.abstractmethod - def horzcat(x: npt.ArrayLike) -> npt.ArrayLike: + def horzcat(self, x: npt.ArrayLike) -> npt.ArrayLike: """ Args: x (npt.ArrayLike): elements @@ -129,11 +129,11 @@ def horzcat(x: npt.ArrayLike) -> npt.ArrayLike: pass @abc.abstractmethod - def mtimes(x: npt.ArrayLike, y: npt.ArrayLike) -> npt.ArrayLike: + def mtimes(self, x: npt.ArrayLike, y: npt.ArrayLike) -> npt.ArrayLike: pass @abc.abstractmethod - def sin(x: npt.ArrayLike) -> npt.ArrayLike: + def sin(self, x: npt.ArrayLike) -> npt.ArrayLike: """ Args: x (npt.ArrayLike): angle value @@ -144,7 +144,7 @@ def sin(x: npt.ArrayLike) -> npt.ArrayLike: pass @abc.abstractmethod - def cos(x: npt.ArrayLike) -> npt.ArrayLike: + def cos(self, x: npt.ArrayLike) -> npt.ArrayLike: """ Args: x (npt.ArrayLike): angle value @@ -155,7 +155,7 @@ def cos(x: npt.ArrayLike) -> npt.ArrayLike: pass @abc.abstractmethod - def skew(x): + def skew(self, x): pass def R_from_axis_angle(self, axis: npt.ArrayLike, q: npt.ArrayLike) -> npt.ArrayLike: From fbeb8e4a9183c4a777833c5d2781c3e1efedd83c Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 12:49:57 +0100 Subject: [PATCH 19/32] Properly using Pose objects in parametric joint and links --- .../parametric_factories/parametric_joint.py | 36 +++---- .../parametric_factories/parametric_link.py | 95 +++++++------------ 2 files changed, 49 insertions(+), 82 deletions(-) diff --git a/src/adam/parametric/model/parametric_factories/parametric_joint.py b/src/adam/parametric/model/parametric_factories/parametric_joint.py index 05b18618..7198e286 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_joint.py +++ b/src/adam/parametric/model/parametric_factories/parametric_joint.py @@ -45,20 +45,14 @@ def modify(self, parent_joint_offset: npt.ArrayLike): length = self.parent_parametric.get_principal_length_parametric() # Ack for avoiding depending on casadi - vo = self.parent_parametric.inertial.origin[2] - xyz_rpy = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - xyz_rpy[0] = self.joint.origin.xyz[0] - xyz_rpy[1] = self.joint.origin.xyz[1] - xyz_rpy[2] = self.joint.origin.xyz[2] - xyz_rpy[3] = self.joint.origin.rpy[0] - xyz_rpy[4] = self.joint.origin.rpy[1] - xyz_rpy[5] = self.joint.origin.rpy[2] - - if xyz_rpy[2] < 0: - xyz_rpy[2] = -length + parent_joint_offset - self.offset + vo = self.parent_parametric.inertial.origin.xyz[2] + modified = self.joint.origin + + if modified.xyz[2] < 0: + modified.xyz[2] = -length + parent_joint_offset - self.offset else: - xyz_rpy[2] = vo + length / 2 - self.offset - return xyz_rpy + modified.xyz[2] = vo + length / 2 - self.offset + return modified def homogeneous(self, q: npt.ArrayLike) -> npt.ArrayLike: """ @@ -70,10 +64,10 @@ def homogeneous(self, q: npt.ArrayLike) -> npt.ArrayLike: """ o = self.math.factory.zeros(3) - o[0] = self.origin[0] - o[1] = self.origin[1] - o[2] = self.origin[2] - rpy = self.origin[3:] + o[0] = self.origin.xyz[0] + o[1] = self.origin.xyz[1] + o[2] = self.origin.xyz[2] + rpy = self.origin.rpy if self.type == "fixed": return self.math.H_from_Pos_RPY(o, rpy) @@ -101,15 +95,15 @@ def spatial_transform(self, q: npt.ArrayLike) -> npt.ArrayLike: npt.ArrayLike: spatial transform of the joint given q """ if self.type == "fixed": - return self.math.X_fixed_joint(self.origin[:3], self.origin[3:]) + return self.math.X_fixed_joint(self.origin.xyz, self.origin.rpy) elif self.type in ["revolute", "continuous"]: return self.math.X_revolute_joint( - self.origin[:3], self.origin[3:], self.axis, q + self.origin.xyz, self.origin.rpy, self.axis, q ) elif self.type in ["prismatic"]: return self.math.X_prismatic_joint( - self.origin[:3], - self.origin[3:], + self.origin.xyz, + self.origin.rpy, self.axis, q, ) diff --git a/src/adam/parametric/model/parametric_factories/parametric_link.py b/src/adam/parametric/model/parametric_factories/parametric_link.py index 48630145..5d08fb52 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_link.py +++ b/src/adam/parametric/model/parametric_factories/parametric_link.py @@ -7,21 +7,7 @@ from adam.model import Link import math -from adam.model.abc_factories import Inertial - - -class I_parametric: - """Inertia values parametric""" - - def __init__(self) -> None: - self.ixx = 0.0 - self.ixy = 0.0 - self.ixz = 0.0 - self.iyx = 0.0 - self.iyy = 0.0 - self.iyz = 0.0 - self.izz = 0.0 - self.ixz = 0.0 +from adam.model.abc_factories import Inertia, Inertial class Geometry(Enum): @@ -61,10 +47,9 @@ def __init__( self.link_offset = self.compute_offset() (self.volume, self.visual_data_new) = self.compute_volume() self.mass = self.compute_mass() - self.I = self.compute_inertia_parametric() self.inertial = Inertial(self.mass) self.inertial.mass = self.mass - self.inertial.inertia = self.I + self.inertial.inertia = self.compute_inertia_parametric() self.inertial.origin = self.modify_origin() self.update_visuals() @@ -144,12 +129,12 @@ def get_geometry(visual_obj): Returns: (Geometry, urdf geometry): the geometry of the link and the related urdf object """ - if type(visual_obj.geometry) is urdf_parser_py.urdf.Box: - return (Geometry.BOX, visual_obj.geometry) - if type(visual_obj.geometry) is urdf_parser_py.urdf.Cylinder: - return (Geometry.CYLINDER, visual_obj.geometry) - if type(visual_obj.geometry) is urdf_parser_py.urdf.Sphere: - return (Geometry.SPHERE, visual_obj.geometry) + if isinstance(visual_obj.geometry, urdf_parser_py.urdf.Box): + return Geometry.BOX, visual_obj.geometry + if isinstance(visual_obj.geometry, urdf_parser_py.urdf.Cylinder): + return Geometry.CYLINDER, visual_obj.geometry + if isinstance(visual_obj.geometry, urdf_parser_py.urdf.Sphere): + return Geometry.SPHERE, visual_obj.geometry def compute_volume(self): """ @@ -157,15 +142,14 @@ def compute_volume(self): (npt.ArrayLike, npt.ArrayLike): the volume and the dimension parametric """ volume = 0.0 + visual_data_new = [0.0, 0.0, 0.0] """Modifies a link's volume by a given multiplier, in a manner that is logical with the link's geometry""" if self.geometry_type == Geometry.BOX: - visual_data_new = [0.0, 0.0, 0.0] visual_data_new[0] = self.visual_data.size[0] # * self.length_multiplier[0] visual_data_new[1] = self.visual_data.size[1] # * self.length_multiplier[1] visual_data_new[2] = self.visual_data.size[2] * self.length_multiplier volume = visual_data_new[0] * visual_data_new[1] * visual_data_new[2] elif self.geometry_type == Geometry.CYLINDER: - visual_data_new = [0.0, 0.0] visual_data_new[0] = self.visual_data.length * self.length_multiplier visual_data_new[1] = self.visual_data.radius # * self.length_multiplier[1] volume = math.pi * visual_data_new[1] ** 2 * visual_data_new[0] @@ -190,27 +174,18 @@ def modify_origin(self): Returns: (npt.ArrayLike): the link origin parametrized """ - origin = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - xyz_rpy = [*self.original_visual.origin.xyz, *self.original_visual.origin.rpy] - v_o = xyz_rpy[2] + origin = self.original_visual.origin + if self.geometry_type == Geometry.SPHERE: + "in case of a sphere the origin of the link does not change" + return origin + + v_o = self.original_visual.origin.xyz[2] length = self.get_principal_length_parametric() if v_o < 0: - origin[2] = self.link_offset - length / 2 - origin[0] = xyz_rpy[0] - origin[1] = xyz_rpy[1] - origin[3] = xyz_rpy[3] - origin[4] = xyz_rpy[4] - origin[5] = xyz_rpy[5] + origin.xyz[2] = self.link_offset - length / 2 else: - origin[2] = length / 2 + self.link_offset - origin[0] = xyz_rpy[0] - origin[1] = xyz_rpy[1] - origin[3] = xyz_rpy[3] - origin[4] = xyz_rpy[4] - origin[5] = xyz_rpy[5] - if self.geometry_type == Geometry.SPHERE: - "in case of a sphere the origin of the framjoint_name_list[0]:link_parametric.JointCharacteristics(0.0295),e does not change" - origin = xyz_rpy + origin.xyz[2] = length / 2 + self.link_offset + return origin def compute_inertia_parametric(self): @@ -219,7 +194,7 @@ def compute_inertia_parametric(self): Inertia Parametric: inertia (ixx, iyy and izz) with the formula that corresponds to the geometry Formulas retrieved from https://en.wikipedia.org/wiki/List_of_moments_of_inertia """ - I = I_parametric() + I = Inertia(ixx=0.0, iyy=0.0, izz=0.0, ixy=0.0, ixz=0.0, iyz=0.0) xyz_rpy = [*self.original_visual.origin.xyz, *self.original_visual.origin.rpy] if self.geometry_type == Geometry.BOX: I.ixx = ( @@ -262,19 +237,17 @@ def compute_inertia_parametric(self): def spatial_inertia(self) -> npt.ArrayLike: """ - Args: - link (Link): Link - Returns: - npt.ArrayLike: the 6x6 inertia matrix expressed at the origin of the link (with rotation) + npt.ArrayLike: the 6x6 inertia matrix expressed at the + origin of the link (with rotation) """ - I = self.I + I = self.inertial.inertia mass = self.mass - o = self.math.factory.zeros(3) - o[0] = self.inertial.origin[0] - o[1] = self.inertial.origin[1] - o[2] = self.inertial.origin[2] - rpy = self.inertial.origin[3:] + o = self.math.factory.zeros(3) # To specify that the components are not floats + o[0] = self.inertial.origin.xyz[0] + o[1] = self.inertial.origin.xyz[1] + o[2] = self.inertial.origin.xyz[2] + rpy = self.inertial.origin.rpy return self.math.spatial_inertial_with_parameters(I, mass, o, rpy) def homogeneous(self) -> npt.ArrayLike: @@ -283,11 +256,11 @@ def homogeneous(self) -> npt.ArrayLike: npt.ArrayLike: the homogeneous transform of the link """ - o = self.math.factory.zeros(3) - o[0] = self.inertial.origin[0] - o[1] = self.inertial.origin[1] - o[2] = self.inertial.origin[2] - rpy = self.inertial.origin[3:] + o = self.math.factory.zeros(3) # To specify that the components are not floats + o[0] = self.inertial.origin.xyz[0] + o[1] = self.inertial.origin.xyz[1] + o[2] = self.inertial.origin.xyz[2] + rpy = self.inertial.origin.rpy return self.math.H_from_Pos_RPY( o, rpy, @@ -296,9 +269,9 @@ def homogeneous(self) -> npt.ArrayLike: def update_visuals(self): if self.geometry_type == Geometry.BOX: self.visuals[0].geometry.size = self.visual_data_new - self.visuals[0].origin.xyz[2] = self.inertial.origin[2] + self.visuals[0].origin.xyz[2] = self.inertial.origin.xyz[2] elif self.geometry_type == Geometry.CYLINDER: self.visuals[0].geometry.length = self.visual_data_new[0] - self.visuals[0].origin.xyz[2] = self.inertial.origin[2] + self.visuals[0].origin.xyz[2] = self.inertial.origin.xyz[2] elif self.geometry_type == Geometry.SPHERE: self.visuals[0].geometry.radius = self.visual_data_new From 70763292d96266a71e370f1c1d1fd27cd641e352 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 12:50:53 +0100 Subject: [PATCH 20/32] Added test for comparison from parametric model to iDynTree --- tests/conversions/test_idyntree_parametric.py | 246 ++++++++++++++++++ 1 file changed, 246 insertions(+) create mode 100644 tests/conversions/test_idyntree_parametric.py diff --git a/tests/conversions/test_idyntree_parametric.py b/tests/conversions/test_idyntree_parametric.py new file mode 100644 index 00000000..734cee5f --- /dev/null +++ b/tests/conversions/test_idyntree_parametric.py @@ -0,0 +1,246 @@ +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import logging +import casadi as cs +import idyntree.swig as idyntree +import numpy as np +import pytest + +import adam.numpy +from adam.parametric.casadi import KinDynComputationsParametric +from adam.parametric.model.parametric_factories.parametric_model import ( + URDFParametricModelFactory, +) +from adam.model.conversions.idyntree import to_idyntree_model +from adam.core.constants import Representations + +from adam.geometry import utils +import tempfile +from git import Repo + +# Getting stickbot urdf file +temp_dir = tempfile.TemporaryDirectory() +git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git" +Repo.clone_from(git_url, temp_dir.name) +model_path = temp_dir.name + "/models/stickBot/model.urdf" + +joints_name_list = [ + "torso_pitch", + "torso_roll", + "torso_yaw", + "l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "l_hip_pitch", + "l_hip_roll", + "l_hip_yaw", + "l_knee", + "l_ankle_pitch", + "l_ankle_roll", + "r_hip_pitch", + "r_hip_roll", + "r_hip_yaw", + "r_knee", + "r_ankle_pitch", + "r_ankle_roll", +] + + +logging.basicConfig(level=logging.DEBUG) +logging.debug("Showing the robot tree.") + + +root_link = "root_link" + +link_name_list = ["chest"] +comp_w_hardware = KinDynComputationsParametric( + model_path, joints_name_list, link_name_list, root_link +) +comp_w_hardware.set_frame_velocity_representation(Representations.MIXED_REPRESENTATION) + +original_density = [ + 628.0724496264945 +] # This is the original density value associated to the chest link, computed as mass/volume +original_length = np.ones(len(link_name_list)) + +# TODO: the following two commands should be moved to a proper function/method +factory = URDFParametricModelFactory( + path=model_path, + math=adam.numpy.numpy_like.SpatialMath(), + links_name_list=link_name_list, + length_multiplier=original_length, + densities=original_density, +) +model = adam.model.Model.build(factory=factory, joints_name_list=joints_name_list) + + +kinDyn = idyntree.KinDynComputations() +kinDyn.loadRobotModel(to_idyntree_model(model)) +kinDyn.setFloatingBase(root_link) +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 +# joints quantities +joints_val = (np.random.rand(n_dofs) - 0.5) * 5 +joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 + +H_b = utils.H_from_Pos_RPY(xyz, rpy) +vb_ = base_vel +s_ = joints_val +s_dot_ = joints_dot_val + +g = np.array([0, 0, -9.80665]) +kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) + + +def test_mass_matrix(): + mass_mx = idyntree.MatrixDynSize() + kinDyn.getFreeFloatingMassMatrix(mass_mx) + mass_mxNumpy = mass_mx.toNumPy() + M_with_hardware = comp_w_hardware.mass_matrix_fun() + mass_test_hardware = cs.DM( + M_with_hardware(H_b, s_, original_length, original_density) + ) + assert mass_mxNumpy - mass_test_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(): + cmm_idyntree = idyntree.MatrixDynSize() + kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) + cmm_idyntreeNumpy = cmm_idyntree.toNumPy() + Jcm_with_hardware = comp_w_hardware.centroidal_momentum_matrix_fun() + Jcm_test_hardware = cs.DM( + Jcm_with_hardware(H_b, s_, original_length, original_density) + ) + assert cmm_idyntreeNumpy - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(): + CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() + com_with_hardware_f = comp_w_hardware.CoM_position_fun() + CoM_hardware = cs.DM( + com_with_hardware_f(H_b, s_, original_length, original_density) + ) + assert CoM_iDynTree - CoM_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_total_mass(): + mass = kinDyn.model().getTotalMass() + mass_hardware_fun = comp_w_hardware.get_total_mass() + mass_hardware = cs.DM(mass_hardware_fun(original_length, original_density)) + assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian(): + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_tot_with_hardware = comp_w_hardware.jacobian_fun("l_sole") + J_test_hardware = cs.DM( + J_tot_with_hardware(H_b, s_, original_length, original_density) + ) + assert iDynNumpyJ_ - J_test_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(): + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_tot_with_hardware = comp_w_hardware.jacobian_fun("head") + J_tot_with_hardware_test = cs.DM( + J_tot_with_hardware(H_b, s_, original_length, original_density) + ) + assert iDynNumpyJ_ - J_tot_with_hardware_test == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(): + Jdotnu = kinDyn.getFrameBiasAcc("l_sole") + Jdot_nu = Jdotnu.toNumPy() + J_dot_hardware = comp_w_hardware.jacobian_dot_fun("l_sole") + J_dot_nu_test2 = cs.DM( + J_dot_hardware( + H_b, joints_val, base_vel, joints_dot_val, original_length, original_density + ) + @ np.concatenate((base_vel, joints_dot_val)) + ) + assert Jdot_nu - J_dot_nu_test2 == pytest.approx(0.0, abs=1e-5) + + +def test_fk(): + H_idyntree = kinDyn.getWorldTransform("l_sole") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + T_with_hardware = comp_w_hardware.forward_kinematics_fun("l_sole") + H_with_hardware_test = cs.DM( + T_with_hardware(H_b, s_, original_length, original_density) + ) + assert H_with_hardware_test[:3, :3] - R_idy2np == pytest.approx(0.0, abs=1e-5) + assert H_with_hardware_test[:3, 3] - p_idy2np == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(): + H_idyntree = kinDyn.getWorldTransform("head") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + T_with_hardware = comp_w_hardware.forward_kinematics_fun("head") + H_with_hardware_test = cs.DM( + T_with_hardware(H_b, s_, original_length, original_density) + ) + assert H_with_hardware_test[:3, :3] - R_idy2np == pytest.approx(0.0, abs=1e-5) + assert H_with_hardware_test[:3, 3] - p_idy2np == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(): + h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(h_iDyn) + h_iDyn_np = np.concatenate( + (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) + ) + + h_with_hardware = comp_w_hardware.bias_force_fun() + h_with_hardware_test = cs.DM( + h_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density) + ) + assert h_with_hardware_test - h_iDyn_np == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + g0 = idyntree.Vector3() + g0.zero() + kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) + C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(C_iDyn) + C_iDyn_np = np.concatenate( + (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) + ) + C_with_hardware = comp_w_hardware.coriolis_term_fun() + C_with_hardware_test = cs.DM( + C_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density) + ) + assert C_with_hardware_test - C_iDyn_np == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(): + base_vel_0 = np.zeros(6) + joints_dot_val_0 = np.zeros(n_dofs) + kinDyn.setRobotState(H_b, joints_val, base_vel_0, joints_dot_val_0, g) + G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(G_iDyn) + G_iDyn_np = np.concatenate( + (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) + ) + G_with_hardware = comp_w_hardware.gravity_term_fun() + G_with_hardware_test = G_with_hardware(H_b, s_, original_length, original_density) + assert G_with_hardware_test - G_iDyn_np == pytest.approx(0.0, abs=1e-4) From 80f54c8293621a930e43263ba6c55b760e9d86ea Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 14:32:00 +0100 Subject: [PATCH 21/32] Added conversion to idyntree to __init__.py --- src/adam/model/conversions/__init__.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/adam/model/conversions/__init__.py b/src/adam/model/conversions/__init__.py index e69de29b..601ab918 100644 --- a/src/adam/model/conversions/__init__.py +++ b/src/adam/model/conversions/__init__.py @@ -0,0 +1 @@ +from .idyntree import to_idyntree_model From 576584d8623b6a4c634ea7bf2c0120f90e343c8b Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 16:13:51 +0100 Subject: [PATCH 22/32] Properly setting the transform of visuals and set joint limits in the idyntree conversion --- src/adam/model/conversions/idyntree.py | 58 ++++++++++++++++++-------- 1 file changed, 40 insertions(+), 18 deletions(-) diff --git a/src/adam/model/conversions/idyntree.py b/src/adam/model/conversions/idyntree.py index 94cb711c..d6df4ab1 100644 --- a/src/adam/model/conversions/idyntree.py +++ b/src/adam/model/conversions/idyntree.py @@ -9,39 +9,55 @@ def to_idyntree_solid_shape( - visuals: urdf_parser_py.urdf.Visual, + visual: urdf_parser_py.urdf.Visual, ) -> idyntree.bindings.SolidShape: """ Args: - visuals (urdf_parser_py.urdf.Visual): the visual to convert + visual (urdf_parser_py.urdf.Visual): the visual to convert Returns: iDynTree.SolidShape: the iDynTree solid shape """ - if isinstance(visuals.geometry, urdf_parser_py.urdf.Box): + visual_position = idyntree.bindings.Position.FromPython(visual.origin.xyz) # noqa + visual_rotation = idyntree.bindings.Rotation.RPY(*visual.origin.rpy) # noqa + visual_transform = idyntree.bindings.Transform() + visual_transform.setRotation(visual_rotation) + visual_transform.setPosition(visual_position) + material = idyntree.bindings.Material(visual.material.name) + if visual.material.color is not None: + color = idyntree.bindings.Vector4() + color[0] = visual.material.color.rgba[0] + color[1] = visual.material.color.rgba[1] + color[2] = visual.material.color.rgba[2] + color[3] = visual.material.color.rgba[3] + material.setColor(color) + if isinstance(visual.geometry, urdf_parser_py.urdf.Box): output = idyntree.bindings.Box() - output.setX(visuals.geometry.size[0]) - output.setY(visuals.geometry.size[1]) - output.setZ(visuals.geometry.size[2]) + output.setX(visual.geometry.size[0]) + output.setY(visual.geometry.size[1]) + output.setZ(visual.geometry.size[2]) + output.setLink_H_geometry(visual_transform) return output - if isinstance(visuals.geometry, urdf_parser_py.urdf.Cylinder): + if isinstance(visual.geometry, urdf_parser_py.urdf.Cylinder): output = idyntree.bindings.Cylinder() - output.setRadius(visuals.geometry.radius) - output.setLength(visuals.geometry.length) + output.setRadius(visual.geometry.radius) + output.setLength(visual.geometry.length) + output.setLink_H_geometry(visual_transform) return output - - if isinstance(visuals.geometry, urdf_parser_py.urdf.Sphere): + if isinstance(visual.geometry, urdf_parser_py.urdf.Sphere): output = idyntree.bindings.Sphere() - output.setRadius(visuals.geometry.radius) + output.setRadius(visual.geometry.radius) + output.setLink_H_geometry(visual_transform) return output - if isinstance(visuals.geometry, urdf_parser_py.urdf.Mesh): + if isinstance(visual.geometry, urdf_parser_py.urdf.Mesh): output = idyntree.bindings.ExternalMesh() - output.setFilename(visuals.geometry.filename) - output.setScale(visuals.geometry.scale) + output.setFilename(visual.geometry.filename) + output.setScale(visual.geometry.scale) + output.setLink_H_geometry(visual_transform) return output raise NotImplementedError( - f"The visual type {visuals.geometry.__class__} is not supported" + f"The visual type {visual.geometry.__class__} is not supported" ) @@ -94,8 +110,6 @@ def to_idyntree_joint( iDynTree.bindings.IJoint: the iDynTree joint """ - # TODO: consider limits - rest_position = idyntree.bindings.Position.FromPython(joint.origin.xyz) # noqa rest_rotation = idyntree.bindings.Rotation.RPY(*joint.origin.rpy) # noqa rest_transform = idyntree.bindings.Transform() @@ -116,12 +130,20 @@ def to_idyntree_joint( output.setAttachedLinks(parent_index, child_index) output.setRestTransform(rest_transform) output.setAxis(axis, child_index, parent_index) + if joint.limit is not None and joint.type == "revolute": + min = joint.limit.lower + max = joint.limit.upper + output.setPosLimits(0, min, max) return output if joint.type in ["prismatic"]: output = idyntree.bindings.PrismaticJoint() output.setAttachedLinks(parent_index, child_index) output.setRestTransform(rest_transform) output.setAxis(axis, child_index, parent_index) + if joint.limit is not None: + min = joint.limit.lower + max = joint.limit.upper + output.setPosLimits(0, min, max) return output NotImplementedError(f"The joint type {joint.type} is not supported") From 4539ebd3a063570b5906476ccd95674be856b4f4 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 16:18:40 +0100 Subject: [PATCH 23/32] Removed check of attribute availability in the conversion to idyntree --- src/adam/model/conversions/idyntree.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/adam/model/conversions/idyntree.py b/src/adam/model/conversions/idyntree.py index d6df4ab1..b446db10 100644 --- a/src/adam/model/conversions/idyntree.py +++ b/src/adam/model/conversions/idyntree.py @@ -171,9 +171,6 @@ def to_idyntree_model(model: Model) -> idyntree.bindings.Model: links_map[node.name] = link_index for i, visuals in enumerate(output_visuals): - # To be removed after the change is in iDynTree - if not hasattr(output.visualSolidShapes(), "clearSingleLinkSolidShapes"): - continue output.visualSolidShapes().clearSingleLinkSolidShapes(i) for visual in visuals: output.visualSolidShapes().addSingleLinkSolidShape(i, visual) From e7ee4279e0f81f61ed5c039bfb40a7fa32ee5052 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 16:32:44 +0100 Subject: [PATCH 24/32] Added Limits type hint and fixed other type warnings --- src/adam/model/abc_factories.py | 12 ++++++++++- src/adam/model/conversions/idyntree.py | 24 ++++++++------------- src/adam/model/model.py | 29 +++++++++++++------------- 3 files changed, 35 insertions(+), 30 deletions(-) diff --git a/src/adam/model/abc_factories.py b/src/adam/model/abc_factories.py index ade55f67..853dbb8a 100644 --- a/src/adam/model/abc_factories.py +++ b/src/adam/model/abc_factories.py @@ -27,6 +27,16 @@ class Inertia: izz: npt.ArrayLike +@dataclasses.dataclass +class Limits: + """Limits class""" + + lower: npt.ArrayLike + upper: npt.ArrayLike + effort: npt.ArrayLike + velocity: npt.ArrayLike + + @dataclasses.dataclass class Joint(abc.ABC): """Base Joint class. You need to fill at least these fields""" @@ -38,7 +48,7 @@ class Joint(abc.ABC): type: str axis: List origin: Pose - limit: List + limit: Limits idx: int """ Abstract base class for all joints. diff --git a/src/adam/model/conversions/idyntree.py b/src/adam/model/conversions/idyntree.py index b446db10..bbd8fd8e 100644 --- a/src/adam/model/conversions/idyntree.py +++ b/src/adam/model/conversions/idyntree.py @@ -18,8 +18,8 @@ def to_idyntree_solid_shape( Returns: iDynTree.SolidShape: the iDynTree solid shape """ - visual_position = idyntree.bindings.Position.FromPython(visual.origin.xyz) # noqa - visual_rotation = idyntree.bindings.Rotation.RPY(*visual.origin.rpy) # noqa + visual_position = idyntree.bindings.Position.FromPython(visual.origin.xyz) + visual_rotation = idyntree.bindings.Rotation.RPY(*visual.origin.rpy) visual_transform = idyntree.bindings.Transform() visual_transform.setRotation(visual_rotation) visual_transform.setPosition(visual_position) @@ -110,8 +110,8 @@ def to_idyntree_joint( iDynTree.bindings.IJoint: the iDynTree joint """ - rest_position = idyntree.bindings.Position.FromPython(joint.origin.xyz) # noqa - rest_rotation = idyntree.bindings.Rotation.RPY(*joint.origin.rpy) # noqa + rest_position = idyntree.bindings.Position.FromPython(joint.origin.xyz) + rest_rotation = idyntree.bindings.Rotation.RPY(*joint.origin.rpy) rest_transform = idyntree.bindings.Transform() rest_transform.setRotation(rest_rotation) rest_transform.setPosition(rest_position) @@ -131,9 +131,7 @@ def to_idyntree_joint( output.setRestTransform(rest_transform) output.setAxis(axis, child_index, parent_index) if joint.limit is not None and joint.type == "revolute": - min = joint.limit.lower - max = joint.limit.upper - output.setPosLimits(0, min, max) + output.setPosLimits(0, joint.limit.lower, joint.limit.upper) return output if joint.type in ["prismatic"]: output = idyntree.bindings.PrismaticJoint() @@ -141,9 +139,7 @@ def to_idyntree_joint( output.setRestTransform(rest_transform) output.setAxis(axis, child_index, parent_index) if joint.limit is not None: - min = joint.limit.lower - max = joint.limit.upper - output.setPosLimits(0, min, max) + output.setPosLimits(0, joint.limit.lower, joint.limit.upper) return output NotImplementedError(f"The joint type {joint.type} is not supported") @@ -182,13 +178,11 @@ def to_idyntree_model(model: Model) -> idyntree.bindings.Model: joint_index = output.addJoint(j.name, joint) assert output.isValidJointIndex(joint_index) - frames_list = [f + "_fixed_joint" for f in model.frames] # noqa + frames_list = [f + "_fixed_joint" for f in model.frames] for name in model.joints: if name in frames_list: - joint = model.joints[name] # noqa - frame_position = idyntree.bindings.Position.FromPython( - joint.origin.xyz # noqa - ) + joint = model.joints[name] + frame_position = idyntree.bindings.Position.FromPython(joint.origin.xyz) frame_transform = idyntree.bindings.Transform() frame_transform.setRotation( idyntree.bindings.Rotation.RPY(*joint.origin.rpy) diff --git a/src/adam/model/model.py b/src/adam/model/model.py index 0f51306f..074c1599 100644 --- a/src/adam/model/model.py +++ b/src/adam/model/model.py @@ -1,5 +1,4 @@ import dataclasses -import pathlib from typing import Dict, List from adam.model.abc_factories import Joint, Link, ModelFactory @@ -12,9 +11,9 @@ class Model: Model class. It describes the robot using links and frames and their connectivity""" name: str - links: List[Link] - frames: List[Link] - joints: List[Joint] + links: Dict[str, Link] + frames: Dict[str, Link] + joints: Dict[str, Joint] tree: Tree NDoF: int actuated_joints: List[str] @@ -35,36 +34,38 @@ def build(factory: ModelFactory, joints_name_list: List[str] = None) -> "Model": Model: the model describing the robot """ - joints = factory.get_joints() - links = factory.get_links() - frames = factory.get_frames() + joints_list = factory.get_joints() + links_list = factory.get_links() + frames_list = factory.get_frames() # if the joints_name_list is None, set it to the list of all the joints that are not fixed if joints_name_list is None: - joints_name_list = [joint.name for joint in joints if joint.type != "fixed"] + joints_name_list = [ + joint.name for joint in joints_list if joint.type != "fixed" + ] print( f"joints_name_list is None. Setting it to the list of all the joints that are not fixed: {joints_name_list}" ) # check if the joints in the list are in the model for joint_str in joints_name_list: - if joint_str not in [joint.name for joint in joints]: + if joint_str not in [joint.name for joint in joints_list]: raise ValueError( f"{joint_str} is not in the robot model. Check the joints_name_list" ) # set idx to the actuated joints for [idx, joint_str] in enumerate(joints_name_list): - for joint in joints: + for joint in joints_list: if joint.name == joint_str: joint.idx = idx - tree = Tree.build_tree(links=links, joints=joints) + tree = Tree.build_tree(links=links_list, joints=joints_list) # generate some useful dict - joints: Dict(str, Joint) = {joint.name: joint for joint in joints} - links: Dict(str, Link) = {link.name: link for link in links} - frames: Dict(str, Link) = {frame.name: frame for frame in frames} + joints: Dict[str, Joint] = {joint.name: joint for joint in joints_list} + links: Dict[str, Link] = {link.name: link for link in links_list} + frames: Dict[str, Link] = {frame.name: frame for frame in frames_list} return Model( name=factory.name, From 66f2273205273f1b0a3e15e9219a2dad4d37eea6 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 16:33:39 +0100 Subject: [PATCH 25/32] Removed unused method with empty start string --- src/adam/model/model.py | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/adam/model/model.py b/src/adam/model/model.py index 074c1599..887b4e7a 100644 --- a/src/adam/model/model.py +++ b/src/adam/model/model.py @@ -120,14 +120,6 @@ def get_total_mass(self) -> float: mass += link.inertial.mass return mass - def get_ordered_link_list(self): - """get the ordered list of the link, based on the direction of the graph - - Returns: - list: ordered link list - """ - return self.tree.get_ordered_nodes_list() - def print_table(self): """print the table that describes the connectivity between the elements. You need rich to use it From 8e25f0287ff3abd20ca709557c4c5f1fa29587a4 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 17:00:16 +0100 Subject: [PATCH 26/32] Return an error if the input link is not supported in the parametric model --- .../parametric/model/parametric_factories/parametric_link.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/adam/parametric/model/parametric_factories/parametric_link.py b/src/adam/parametric/model/parametric_factories/parametric_link.py index 5d08fb52..85a2860b 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_link.py +++ b/src/adam/parametric/model/parametric_factories/parametric_link.py @@ -136,6 +136,10 @@ def get_geometry(visual_obj): if isinstance(visual_obj.geometry, urdf_parser_py.urdf.Sphere): return Geometry.SPHERE, visual_obj.geometry + raise NotImplementedError( + f"The visual type {visual_obj.geometry.__class__} is not supported" + ) + def compute_volume(self): """ Returns: @@ -165,7 +169,6 @@ def compute_mass(self): Returns: (npt.ArrayLike): the link mass """ - mass = 0.0 mass = self.volume * self.densities return mass From fa390b8e7b94f10e293e4bef12666ebbbe5d6a8a Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 17:17:42 +0100 Subject: [PATCH 27/32] Some type hinting fix in tree.py --- src/adam/model/tree.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/adam/model/tree.py b/src/adam/model/tree.py index dc96bfe5..1a934e19 100644 --- a/src/adam/model/tree.py +++ b/src/adam/model/tree.py @@ -11,9 +11,9 @@ class Node: name: str link: Link arcs: List[Joint] - children: List[Link] - parent: Link = None - parent_arc: Joint = None + children: List["Node"] + parent: Link | None = None + parent_arc: Joint | None = None def __hash__(self) -> int: return hash(self.name) @@ -48,7 +48,7 @@ def build_tree(links: List[Link], joints: List[Joint]) -> "Tree": Returns: Tree: the directed tree """ - nodes: Dict(str, Node) = { + nodes: Dict[str, Node] = { l.name: Node( name=l.name, link=l, arcs=[], children=[], parent=None, parent_arc=None ) @@ -71,7 +71,7 @@ def build_tree(links: List[Link], joints: List[Joint]) -> "Tree": raise ValueError("The model has more than one root link") return Tree(nodes, root_link[0]) - def print(self, root) -> str: + def print(self, root): """prints the tree Args: From fa984b1d7bbd069d58c12f8ba002eb20cbdbd908 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 17:18:53 +0100 Subject: [PATCH 28/32] Removed useless member in parametric link --- .../parametric/model/parametric_factories/parametric_joint.py | 2 +- .../parametric/model/parametric_factories/parametric_link.py | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/adam/parametric/model/parametric_factories/parametric_joint.py b/src/adam/parametric/model/parametric_factories/parametric_joint.py index 7198e286..e22973b3 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_joint.py +++ b/src/adam/parametric/model/parametric_factories/parametric_joint.py @@ -20,7 +20,7 @@ def __init__( ) -> None: self.math = math self.name = joint.name - self.parent = parent_link.link.name + self.parent = parent_link.name self.parent_parametric = parent_link self.child = joint.child self.type = joint.joint_type diff --git a/src/adam/parametric/model/parametric_factories/parametric_link.py b/src/adam/parametric/model/parametric_factories/parametric_link.py index 85a2860b..d37f9699 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_link.py +++ b/src/adam/parametric/model/parametric_factories/parametric_link.py @@ -43,7 +43,6 @@ def __init__( self.original_visual = copy.deepcopy(link.visual) self.visuals = [copy.deepcopy(link.visual)] self.geometry_type, self.visual_data = self.get_geometry(self.original_visual) - self.link = link self.link_offset = self.compute_offset() (self.volume, self.visual_data_new) = self.compute_volume() self.mass = self.compute_mass() From bfb9ffc95cd825f8b5d004f8c106abb7fa537327 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 18:41:41 +0100 Subject: [PATCH 29/32] Added method to retrieve the original densities --- .../casadi/computations_parametric.py | 18 +++++++++++++++++- .../parametric/jax/computations_parametric.py | 18 +++++++++++++++++- .../parametric_factories/parametric_link.py | 17 ++++++++++------- .../numpy/computations_parametric.py | 17 ++++++++++++++++- .../pytorch/computations_parametric.py | 19 +++++++++++++++++-- 5 files changed, 77 insertions(+), 12 deletions(-) diff --git a/src/adam/parametric/casadi/computations_parametric.py b/src/adam/parametric/casadi/computations_parametric.py index ef922a36..7cc42656 100644 --- a/src/adam/parametric/casadi/computations_parametric.py +++ b/src/adam/parametric/casadi/computations_parametric.py @@ -1,6 +1,7 @@ # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +from typing import List import casadi as cs import numpy as np @@ -9,7 +10,7 @@ from adam.core import RBDAlgorithms from adam.core.constants import Representations from adam.model import Model -from adam.parametric.model import URDFParametricModelFactory +from adam.parametric.model import URDFParametricModelFactory, ParametricLink class KinDynComputationsParametric: @@ -37,6 +38,7 @@ def __init__( n_param_links = len(links_name_list) self.densities = cs.SX.sym("densities", n_param_links) self.length_multiplier = cs.SX.sym("length_multiplier", n_param_links) + self.links_name_list = links_name_list factory = URDFParametricModelFactory( path=urdfstring, math=math, @@ -258,3 +260,17 @@ def get_total_mass(self) -> cs.Function: return cs.Function( "m", [self.length_multiplier, self.densities], [m], self.f_opts ) + + def get_original_densities(self) -> List[float]: + """Returns the original densities of the links + + Returns: + densities: The original densities + """ + densities = [] + model = self.rbdalgos.model + for name in self.links_name_list: + link = model.links[name] + assert isinstance(link, ParametricLink) + densities.append(link.original_density) + return densities diff --git a/src/adam/parametric/jax/computations_parametric.py b/src/adam/parametric/jax/computations_parametric.py index fb0823c9..483f1129 100644 --- a/src/adam/parametric/jax/computations_parametric.py +++ b/src/adam/parametric/jax/computations_parametric.py @@ -2,6 +2,8 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +from typing import List + import jax.numpy as jnp import numpy as np from jax import grad, jit, vmap @@ -10,7 +12,7 @@ from adam.core.constants import Representations from adam.jax.jax_like import SpatialMath from adam.model import Model -from adam.parametric.model import URDFParametricModelFactory +from adam.parametric.model import URDFParametricModelFactory, ParametricLink class KinDynComputationsParametric: @@ -446,3 +448,17 @@ def get_total_mass( self.rbdalgos.set_frame_velocity_representation(self.representation) self.NDoF = self.rbdalgos.NDoF return self.rbdalgos.get_total_mass() + + def get_original_densities(self) -> List[float]: + """Returns the original densities of the links + + Returns: + densities: The original densities + """ + densities = [] + model = self.rbdalgos.model + for name in self.links_name_list: + link = model.links[name] + assert isinstance(link, ParametricLink) + densities.append(link.original_density) + return densities diff --git a/src/adam/parametric/model/parametric_factories/parametric_link.py b/src/adam/parametric/model/parametric_factories/parametric_link.py index d37f9699..c9ae02be 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_link.py +++ b/src/adam/parametric/model/parametric_factories/parametric_link.py @@ -43,8 +43,12 @@ def __init__( self.original_visual = copy.deepcopy(link.visual) self.visuals = [copy.deepcopy(link.visual)] self.geometry_type, self.visual_data = self.get_geometry(self.original_visual) + original_volume, _ = self.compute_volume(length_multiplier=1.0) + self.original_density = link.inertial.mass / original_volume self.link_offset = self.compute_offset() - (self.volume, self.visual_data_new) = self.compute_volume() + (self.volume, self.visual_data_new) = self.compute_volume( + length_multiplier=self.length_multiplier + ) self.mass = self.compute_mass() self.inertial = Inertial(self.mass) self.inertial.mass = self.mass @@ -139,7 +143,7 @@ def get_geometry(visual_obj): f"The visual type {visual_obj.geometry.__class__} is not supported" ) - def compute_volume(self): + def compute_volume(self, length_multiplier): """ Returns: (npt.ArrayLike, npt.ArrayLike): the volume and the dimension parametric @@ -150,21 +154,20 @@ def compute_volume(self): if self.geometry_type == Geometry.BOX: visual_data_new[0] = self.visual_data.size[0] # * self.length_multiplier[0] visual_data_new[1] = self.visual_data.size[1] # * self.length_multiplier[1] - visual_data_new[2] = self.visual_data.size[2] * self.length_multiplier + visual_data_new[2] = self.visual_data.size[2] * length_multiplier volume = visual_data_new[0] * visual_data_new[1] * visual_data_new[2] elif self.geometry_type == Geometry.CYLINDER: - visual_data_new[0] = self.visual_data.length * self.length_multiplier + visual_data_new[0] = self.visual_data.length * length_multiplier visual_data_new[1] = self.visual_data.radius # * self.length_multiplier[1] volume = math.pi * visual_data_new[1] ** 2 * visual_data_new[0] elif self.geometry_type == Geometry.SPHERE: - visual_data_new = self.visual_data.radius * self.length_multiplier + visual_data_new = self.visual_data.radius * length_multiplier volume = 4 * math.pi * visual_data_new**3 / 3 return volume, visual_data_new - """Function that computes the mass starting from the densities, the length multiplier and the link""" - def compute_mass(self): """ + Function that computes the mass starting from the densities, and the link volume Returns: (npt.ArrayLike): the link mass """ diff --git a/src/adam/parametric/numpy/computations_parametric.py b/src/adam/parametric/numpy/computations_parametric.py index e4338251..9bb685a1 100644 --- a/src/adam/parametric/numpy/computations_parametric.py +++ b/src/adam/parametric/numpy/computations_parametric.py @@ -3,11 +3,12 @@ # GNU Lesser General Public License v2.1 or any later version. import numpy as np +from typing import List from adam.core.rbd_algorithms import RBDAlgorithms from adam.core.constants import Representations from adam.model import Model -from adam.parametric.model import URDFParametricModelFactory +from adam.parametric.model import URDFParametricModelFactory, ParametricLink from adam.numpy.numpy_like import SpatialMath @@ -441,3 +442,17 @@ def get_total_mass( self.rbdalgos.set_frame_velocity_representation(self.representation) self.NDoF = model.NDoF return self.rbdalgos.get_total_mass() + + def get_original_densities(self) -> List[float]: + """Returns the original densities of the links + + Returns: + densities: The original densities + """ + densities = [] + model = self.rbdalgos.model + for name in self.links_name_list: + link = model.links[name] + assert isinstance(link, ParametricLink) + densities.append(link.original_density) + return densities diff --git a/src/adam/parametric/pytorch/computations_parametric.py b/src/adam/parametric/pytorch/computations_parametric.py index 531124f7..3bc44610 100644 --- a/src/adam/parametric/pytorch/computations_parametric.py +++ b/src/adam/parametric/pytorch/computations_parametric.py @@ -4,11 +4,12 @@ import numpy as np import torch +from typing import List from adam.core.rbd_algorithms import RBDAlgorithms from adam.core.constants import Representations -from adam.model import Model, URDFModelFactory -from adam.parametric.model import URDFParametricModelFactory +from adam.model import Model +from adam.parametric.model import URDFParametricModelFactory, ParametricLink from adam.pytorch.torch_like import SpatialMath @@ -445,3 +446,17 @@ def get_total_mass( self.rbdalgos.set_frame_velocity_representation(self.representation) self.NDoF = self.rbdalgos.NDoF return self.rbdalgos.get_total_mass() + + def get_original_densities(self) -> List[float]: + """Returns the original densities of the links + + Returns: + densities: The original densities + """ + densities = [] + model = self.rbdalgos.model + for name in self.links_name_list: + link = model.links[name] + assert isinstance(link, ParametricLink) + densities.append(link.original_density) + return densities From 045922ee541b37b1b0c674b9aec61a2b4fcd61f0 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 18:46:46 +0100 Subject: [PATCH 30/32] Use Union in Tree type hint to be compatible with python 3.8 --- src/adam/model/tree.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/adam/model/tree.py b/src/adam/model/tree.py index 1a934e19..73d36210 100644 --- a/src/adam/model/tree.py +++ b/src/adam/model/tree.py @@ -12,8 +12,8 @@ class Node: link: Link arcs: List[Joint] children: List["Node"] - parent: Link | None = None - parent_arc: Joint | None = None + parent: Union[Link, None] = None + parent_arc: Union[Joint, None] = None def __hash__(self) -> int: return hash(self.name) From 67a9e7d65d91911fe5b268f8d0e29847961d8f21 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 12 Mar 2024 12:07:21 +0100 Subject: [PATCH 31/32] Set minimum idyntree version in CI --- ci_env.yml | 4 ++-- ci_env_win.yml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ci_env.yml b/ci_env.yml index 50ae6062..e3c5de4e 100644 --- a/ci_env.yml +++ b/ci_env.yml @@ -15,7 +15,7 @@ dependencies: - pytest - pytest-repeat - icub-models - - idyntree + - idyntree >=11.0.0 - gitpython - jax - - pytorch \ No newline at end of file + - pytorch diff --git a/ci_env_win.yml b/ci_env_win.yml index d8a682ca..0f76e8cb 100644 --- a/ci_env_win.yml +++ b/ci_env_win.yml @@ -15,5 +15,5 @@ dependencies: - pytest - pytest-repeat - icub-models - - idyntree + - idyntree >=11.0.0 - gitpython From 5d2252d34a97f6ee1a4f7c4462a16bb775c7cd6c Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 12 Mar 2024 14:31:15 +0100 Subject: [PATCH 32/32] improved the documentation of the get_original_densities methods --- src/adam/parametric/casadi/computations_parametric.py | 4 ++-- src/adam/parametric/jax/computations_parametric.py | 4 ++-- src/adam/parametric/numpy/computations_parametric.py | 4 ++-- src/adam/parametric/pytorch/computations_parametric.py | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/adam/parametric/casadi/computations_parametric.py b/src/adam/parametric/casadi/computations_parametric.py index 7cc42656..37459821 100644 --- a/src/adam/parametric/casadi/computations_parametric.py +++ b/src/adam/parametric/casadi/computations_parametric.py @@ -262,10 +262,10 @@ def get_total_mass(self) -> cs.Function: ) def get_original_densities(self) -> List[float]: - """Returns the original densities of the links + """Returns the original densities of the parametric links Returns: - densities: The original densities + densities: The original densities of the parametric links """ densities = [] model = self.rbdalgos.model diff --git a/src/adam/parametric/jax/computations_parametric.py b/src/adam/parametric/jax/computations_parametric.py index 483f1129..42865f00 100644 --- a/src/adam/parametric/jax/computations_parametric.py +++ b/src/adam/parametric/jax/computations_parametric.py @@ -450,10 +450,10 @@ def get_total_mass( return self.rbdalgos.get_total_mass() def get_original_densities(self) -> List[float]: - """Returns the original densities of the links + """Returns the original densities of the parametric links Returns: - densities: The original densities + densities: The original densities of the parametric links """ densities = [] model = self.rbdalgos.model diff --git a/src/adam/parametric/numpy/computations_parametric.py b/src/adam/parametric/numpy/computations_parametric.py index 9bb685a1..66c5be91 100644 --- a/src/adam/parametric/numpy/computations_parametric.py +++ b/src/adam/parametric/numpy/computations_parametric.py @@ -444,10 +444,10 @@ def get_total_mass( return self.rbdalgos.get_total_mass() def get_original_densities(self) -> List[float]: - """Returns the original densities of the links + """Returns the original densities of the parametric links Returns: - densities: The original densities + densities: The original densities of the parametric links """ densities = [] model = self.rbdalgos.model diff --git a/src/adam/parametric/pytorch/computations_parametric.py b/src/adam/parametric/pytorch/computations_parametric.py index 3bc44610..03c2f80b 100644 --- a/src/adam/parametric/pytorch/computations_parametric.py +++ b/src/adam/parametric/pytorch/computations_parametric.py @@ -448,10 +448,10 @@ def get_total_mass( return self.rbdalgos.get_total_mass() def get_original_densities(self) -> List[float]: - """Returns the original densities of the links + """Returns the original densities of the parametric links Returns: - densities: The original densities + densities: The original densities of the parametric links """ densities = [] model = self.rbdalgos.model