diff --git a/docs/conf.py b/docs/conf.py index c64063f6..e04e1750 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -18,7 +18,7 @@ # https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information project = "adam" -copyright = "2021, Artificial and Mechanical Intelligence Lab" +copyright = "Artificial and Mechanical Intelligence Lab" author = "Artificial and Mechanical Intelligence Lab" # get release from git tag diff --git a/setup.py b/setup.py index b9394a34..d7b82f52 100644 --- a/setup.py +++ b/setup.py @@ -1,6 +1,5 @@ -# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# Copyright (C) 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 setuptools import setup diff --git a/src/adam/__init__.py b/src/adam/__init__.py index 6d51bece..3fb0daba 100644 --- a/src/adam/__init__.py +++ b/src/adam/__init__.py @@ -1,5 +1,3 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. from adam.core import Representations diff --git a/src/adam/casadi/__init__.py b/src/adam/casadi/__init__.py index 1c3b0795..583c07a7 100644 --- a/src/adam/casadi/__init__.py +++ b/src/adam/casadi/__init__.py @@ -1,6 +1,4 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. from .casadi_like import CasadiLike from .computations import KinDynComputations diff --git a/src/adam/casadi/casadi_like.py b/src/adam/casadi/casadi_like.py index b8a5be41..9feac024 100644 --- a/src/adam/casadi/casadi_like.py +++ b/src/adam/casadi/casadi_like.py @@ -1,6 +1,4 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. from dataclasses import dataclass from typing import Union diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index 8b0dc66d..ff67fda2 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -1,10 +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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. import casadi as cs import numpy as np -from typing import Union from adam.casadi.casadi_like import SpatialMath from adam.core import RBDAlgorithms @@ -13,15 +10,13 @@ class KinDynComputations: - """This is a small class that retrieves robot quantities represented in a symbolic fashion using CasADi - in mixed representation, for Floating Base systems - as humanoid robots. - """ + """Class that retrieves robot quantities using CasADi for Floating Base systems.""" def __init__( self, urdfstring: str, joints_name_list: list = None, - root_link: str = "root_link", + root_link: str = None, gravity: np.array = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]), f_opts: dict = dict(jit=False, jit_options=dict(flags="-Ofast"), cse=True), ) -> None: @@ -29,7 +24,7 @@ def __init__( Args: urdfstring (str): either path or string of the urdf joints_name_list (list): list of the actuated joints - root_link (str, optional): the first link. Defaults to 'root_link'. + root_link (str, optional): Deprecated. The root link is automatically chosen as the link with no parent in the URDF. Defaults to None. """ math = SpatialMath() factory = URDFModelFactory(path=urdfstring, math=math) @@ -38,6 +33,10 @@ def __init__( self.NDoF = self.rbdalgos.NDoF self.g = gravity self.f_opts = f_opts + if root_link is not None: + raise DeprecationWarning( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF" + ) def set_frame_velocity_representation( self, representation: Representations @@ -57,7 +56,7 @@ def mass_matrix_fun(self) -> cs.Function: """ base_transform = cs.SX.sym("H", 4, 4) joint_positions = cs.SX.sym("s", self.NDoF) - [M, _] = self.rbdalgos.crba(base_transform, joint_positions) + M, _ = self.rbdalgos.crba(base_transform, joint_positions) return cs.Function( "M", [base_transform, joint_positions], [M.array], self.f_opts ) @@ -70,7 +69,7 @@ def centroidal_momentum_matrix_fun(self) -> cs.Function: """ base_transform = cs.SX.sym("H", 4, 4) joint_positions = cs.SX.sym("s", self.NDoF) - [_, Jcm] = self.rbdalgos.crba(base_transform, joint_positions) + _, Jcm = self.rbdalgos.crba(base_transform, joint_positions) return cs.Function( "Jcm", [base_transform, joint_positions], [Jcm.array], self.f_opts ) @@ -228,55 +227,51 @@ def get_total_mass(self) -> float: """ return self.rbdalgos.get_total_mass() - def mass_matrix( - self, base_transform: Union[cs.SX, cs.DM], joint_positions: Union[cs.SX, cs.DM] - ): + def mass_matrix(self, base_transform: cs.SX, joint_positions: cs.SX): """Returns the Mass Matrix functions computed the CRBA Args: - base_transform (Union[cs.SX, cs.DM]): The homogenous transform from base to world frame - joint_positions (Union[cs.SX, cs.DM]): The joints position + base_transform (cs.SX): The homogenous transform from base to world frame + joint_positions (cs.SX): The joints position Returns: - M (Union[cs.SX, cs.DM]): Mass Matrix + M (cs.SX): Mass Matrix """ if isinstance(base_transform, cs.MX) and isinstance(joint_positions, cs.MX): raise ValueError( "You are using casadi MX. Please use the function KinDynComputations.mass_matrix_fun()" ) - [M, _] = self.rbdalgos.crba(base_transform, joint_positions) + M, _ = self.rbdalgos.crba(base_transform, joint_positions) return M.array - def centroidal_momentum_matrix( - self, base_transform: Union[cs.SX, cs.DM], joint_positions: Union[cs.SX, cs.DM] - ): + def centroidal_momentum_matrix(self, base_transform: cs.SX, joint_positions: cs.SX): """Returns the Centroidal Momentum Matrix functions computed the CRBA Args: - base_transform (Union[cs.SX, cs.DM]): The homogenous transform from base to world frame - joint_positions (Union[cs.SX, cs.DM]): The joints position + base_transform (cs.SX): The homogenous transform from base to world frame + joint_positions (cs.SX): The joints position Returns: - Jcc (Union[cs.SX, cs.DM]): Centroidal Momentum matrix + Jcc (cs.SX): Centroidal Momentum matrix """ if isinstance(base_transform, cs.MX) and isinstance(joint_positions, cs.MX): raise ValueError( "You are using casadi MX. Please use the function KinDynComputations.centroidal_momentum_matrix_fun()" ) - [_, Jcm] = self.rbdalgos.crba(base_transform, joint_positions) + _, Jcm = self.rbdalgos.crba(base_transform, joint_positions) return Jcm.array - def relative_jacobian(self, frame: str, joint_positions: Union[cs.SX, cs.DM]): + def relative_jacobian(self, frame: str, joint_positions: cs.SX): """Returns the Jacobian between the root link and a specified frame frames Args: frame (str): The tip of the chain - joint_positions (Union[cs.SX, cs.DM]): The joints position + joint_positions (cs.SX): The joints position Returns: - J (Union[cs.SX, cs.DM]): The Jacobian between the root and the frame + J (cs.SX): The Jacobian between the root and the frame """ if isinstance(joint_positions, cs.MX): raise ValueError( @@ -288,22 +283,22 @@ def relative_jacobian(self, frame: str, joint_positions: Union[cs.SX, cs.DM]): def jacobian_dot( self, frame: str, - base_transform: Union[cs.SX, cs.DM], - joint_positions: Union[cs.SX, cs.DM], - base_velocity: Union[cs.SX, cs.DM], - joint_velocities: Union[cs.SX, cs.DM], - ) -> Union[cs.SX, cs.DM]: + base_transform: cs.SX, + joint_positions: cs.SX, + base_velocity: cs.SX, + joint_velocities: cs.SX, + ) -> cs.SX: """Returns the Jacobian derivative relative to the specified frame Args: frame (str): The frame to which the jacobian will be computed - base_transform (Union[cs.SX, cs.DM]): The homogenous transform from base to world frame - joint_positions (Union[cs.SX, cs.DM]): The joints position - base_velocity (Union[cs.SX, cs.DM]): The base velocity in mixed representation - joint_velocities (Union[cs.SX, cs.DM]): The joint velocities + base_transform (cs.SX): The homogenous transform from base to world frame + joint_positions (cs.SX): The joints position + base_velocity (cs.SX): The base velocity in mixed representation + joint_velocities (cs.SX): The joint velocities Returns: - Jdot (Union[cs.SX, cs.DM]): The Jacobian derivative relative to the frame + Jdot (cs.SX): The Jacobian derivative relative to the frame """ if ( isinstance(base_transform, cs.MX) @@ -321,18 +316,18 @@ def jacobian_dot( def forward_kinematics( self, frame: str, - base_transform: Union[cs.SX, cs.DM], - joint_positions: Union[cs.SX, cs.DM], + base_transform: cs.SX, + joint_positions: cs.SX, ): """Computes the forward kinematics relative to the specified frame Args: frame (str): The frame to which the fk will be computed - base_transform (Union[cs.SX, cs.DM]): The homogenous transform from base to world frame - joint_positions (Union[cs.SX, cs.DM]): The joints position + base_transform (cs.SX): The homogenous transform from base to world frame + joint_positions (cs.SX): The joints position Returns: - H (Union[cs.SX, cs.DM]): The fk represented as Homogenous transformation matrix + H (cs.SX): The fk represented as Homogenous transformation matrix """ if isinstance(base_transform, cs.MX) and isinstance(joint_positions, cs.MX): raise ValueError( @@ -347,12 +342,12 @@ def jacobian(self, frame: str, base_transform, joint_positions): """Returns the Jacobian relative to the specified frame Args: - base_transform (Union[cs.SX, cs.DM]): The homogenous transform from base to world frame - s (Union[cs.SX, cs.DM]): The joints position + base_transform (cs.SX): The homogenous transform from base to world frame + s (cs.SX): The joints position frame (str): The frame to which the jacobian will be computed Returns: - J_tot (Union[cs.SX, cs.DM]): The Jacobian relative to the frame + J_tot (cs.SX): The Jacobian relative to the frame """ if isinstance(base_transform, cs.MX) and isinstance(joint_positions, cs.MX): raise ValueError( @@ -363,22 +358,22 @@ def jacobian(self, frame: str, base_transform, joint_positions): def bias_force( self, - base_transform: Union[cs.SX, cs.DM], - joint_positions: Union[cs.SX, cs.DM], - base_velocity: Union[cs.SX, cs.DM], - joint_velocities: Union[cs.SX, cs.DM], - ) -> Union[cs.SX, cs.DM]: + base_transform: cs.SX, + joint_positions: cs.SX, + base_velocity: cs.SX, + joint_velocities: cs.SX, + ) -> cs.SX: """Returns the bias force of the floating-base dynamics equation, using a reduced RNEA (no acceleration and external forces) Args: - base_transform (Union[cs.SX, cs.DM]): The homogenous transform from base to world frame - joint_positions (Union[cs.SX, cs.DM]): The joints position - base_velocity (Union[cs.SX, cs.DM]): The base velocity in mixed representation - joint_velocities (Union[cs.SX, cs.DM]): The joints velocity + base_transform (cs.SX): The homogenous transform from base to world frame + joint_positions (cs.SX): The joints position + base_velocity (cs.SX): The base velocity in mixed representation + joint_velocities (cs.SX): The joints velocity Returns: - h (Union[cs.SX, cs.DM]): the bias force + h (cs.SX): the bias force """ if ( isinstance(base_transform, cs.MX) @@ -396,22 +391,22 @@ def bias_force( def coriolis_term( self, - base_transform: Union[cs.SX, cs.DM], - joint_positions: Union[cs.SX, cs.DM], - base_velocity: Union[cs.SX, cs.DM], - joint_velocities: Union[cs.SX, cs.DM], - ) -> Union[cs.SX, cs.DM]: + base_transform: cs.SX, + joint_positions: cs.SX, + base_velocity: cs.SX, + joint_velocities: cs.SX, + ) -> cs.SX: """Returns the coriolis term of the floating-base dynamics equation, using a reduced RNEA (no acceleration and external forces) Args: - base_transform (Union[cs.SX, cs.DM]): The homogenous transform from base to world frame - joint_positions (Union[cs.SX, cs.DM]): The joints position - base_velocity (Union[cs.SX, cs.DM]): The base velocity in mixed representation - joint_velocities (Union[cs.SX, cs.DM]): The joints velocity + base_transform (cs.SX): The homogenous transform from base to world frame + joint_positions (cs.SX): The joints position + base_velocity (cs.SX): The base velocity in mixed representation + joint_velocities (cs.SX): The joints velocity Returns: - C (Union[cs.SX, cs.DM]): the Coriolis term + C (cs.SX): the Coriolis term """ if ( isinstance(base_transform, cs.MX) @@ -431,18 +426,16 @@ def coriolis_term( np.zeros(6), ).array - def gravity_term( - self, base_transform: Union[cs.SX, cs.DM], joint_positions: Union[cs.SX, cs.DM] - ) -> Union[cs.SX, cs.DM]: + def gravity_term(self, base_transform: cs.SX, joint_positions: cs.SX) -> cs.SX: """Returns the gravity term of the floating-base dynamics equation, using a reduced RNEA (no acceleration and external forces) Args: - base_transform (Union[cs.SX, cs.DM]): The homogenous transform from base to world frame - joint_positions (Union[cs.SX, cs.DM]): The joints position + base_transform (cs.SX): The homogenous transform from base to world frame + joint_positions (cs.SX): The joints position Returns: - G (Union[cs.SX, cs.DM]): the gravity term + G (cs.SX): the gravity term """ if isinstance(base_transform, cs.MX) and isinstance(joint_positions, cs.MX): raise ValueError( @@ -457,17 +450,15 @@ def gravity_term( self.g, ).array - def CoM_position( - self, base_transform: Union[cs.SX, cs.DM], joint_positions: Union[cs.SX, cs.DM] - ) -> Union[cs.SX, cs.DM]: + def CoM_position(self, base_transform: cs.SX, joint_positions: cs.SX) -> cs.SX: """Returns the CoM positon Args: - base_transform (Union[cs.SX, cs.DM]): The homogenous transform from base to world frame - joint_positions (Union[cs.SX, cs.DM]): The joints position + base_transform (cs.SX): The homogenous transform from base to world frame + joint_positions (cs.SX): The joints position Returns: - CoM (Union[cs.SX, cs.DM]): The CoM position + CoM (cs.SX): The CoM position """ if isinstance(base_transform, cs.MX) and isinstance(joint_positions, cs.MX): raise ValueError( diff --git a/src/adam/core/__init__.py b/src/adam/core/__init__.py index b8a1addc..18b8d98c 100644 --- a/src/adam/core/__init__.py +++ b/src/adam/core/__init__.py @@ -1,6 +1,4 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. from .constants import Representations from .rbd_algorithms import RBDAlgorithms diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index b364daaf..4c67d980 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -1,6 +1,5 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. + import numpy.typing as npt from adam.core.constants import Representations diff --git a/src/adam/jax/__init__.py b/src/adam/jax/__init__.py index 70be81d8..87ca661e 100644 --- a/src/adam/jax/__init__.py +++ b/src/adam/jax/__init__.py @@ -1,6 +1,5 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. + from .computations import KinDynComputations from .jax_like import JaxLike diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index 25ad6adc..82d3611f 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -1,6 +1,4 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. import jax.numpy as jnp import numpy as np @@ -18,14 +16,14 @@ def __init__( self, urdfstring: str, joints_name_list: list = None, - root_link: str = "root_link", + root_link: str = None, gravity: np.array = jnp.array([0, 0, -9.80665, 0, 0, 0]), ) -> None: """ Args: urdfstring (str): either path or string of the urdf joints_name_list (list): list of the actuated joints - root_link (str, optional): the first link. Defaults to 'root_link'. + root_link (str, optional): Deprecated. The root link is automatically chosen as the link with no parent in the URDF. Defaults to None. """ math = SpatialMath() factory = URDFModelFactory(path=urdfstring, math=math) @@ -33,6 +31,10 @@ def __init__( self.rbdalgos = RBDAlgorithms(model=model, math=math) self.NDoF = self.rbdalgos.NDoF self.g = gravity + if root_link is not None: + raise DeprecationWarning( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF" + ) def set_frame_velocity_representation( self, representation: Representations diff --git a/src/adam/jax/jax_like.py b/src/adam/jax/jax_like.py index 23ddc2a1..849968ce 100644 --- a/src/adam/jax/jax_like.py +++ b/src/adam/jax/jax_like.py @@ -1,6 +1,5 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. + from dataclasses import dataclass from typing import Union diff --git a/src/adam/model/__init__.py b/src/adam/model/__init__.py index 0426e317..3c23a54a 100644 --- a/src/adam/model/__init__.py +++ b/src/adam/model/__init__.py @@ -1,4 +1,4 @@ -from .abc_factories import Joint, Link, ModelFactory, Inertial, Pose +from .abc_factories import Inertial, Joint, Link, ModelFactory, Pose from .model import Model from .std_factories.std_joint import StdJoint from .std_factories.std_link import StdLink diff --git a/src/adam/model/abc_factories.py b/src/adam/model/abc_factories.py index 4720588f..576ad665 100644 --- a/src/adam/model/abc_factories.py +++ b/src/adam/model/abc_factories.py @@ -1,6 +1,5 @@ import abc import dataclasses -from typing import List import numpy.typing as npt @@ -11,8 +10,8 @@ class Pose: """Pose class""" - xyz: List - rpy: List + xyz: list + rpy: list @dataclasses.dataclass @@ -46,7 +45,7 @@ class Joint(abc.ABC): parent: str child: str type: str - axis: List + axis: list origin: Pose limit: Limits idx: int @@ -126,9 +125,9 @@ class Link(abc.ABC): math: SpatialMath name: str - visuals: List + visuals: list inertial: Inertial - collisions: List + collisions: list @abc.abstractmethod def spatial_inertia(self) -> npt.ArrayLike: @@ -178,25 +177,25 @@ def build_joint(self) -> Joint: pass @abc.abstractmethod - def get_links(self) -> List[Link]: + def get_links(self) -> list[Link]: """ Returns: - List[Link]: the list of the link + list[Link]: the list of the link """ pass @abc.abstractmethod - def get_frames(self) -> List[Link]: + def get_frames(self) -> list[Link]: """ Returns: - List[Link]: the list of the frames + list[Link]: the list of the frames """ pass @abc.abstractmethod - def get_joints(self) -> List[Joint]: + def get_joints(self) -> list[Joint]: """ Returns: - List[Joint]: the list of the joints + list[Joint]: the list of the joints """ pass diff --git a/src/adam/model/conversions/idyntree.py b/src/adam/model/conversions/idyntree.py index bbd8fd8e..e4a30892 100644 --- a/src/adam/model/conversions/idyntree.py +++ b/src/adam/model/conversions/idyntree.py @@ -1,11 +1,9 @@ import idyntree.bindings import numpy as np import urdf_parser_py.urdf -from typing import List - +from adam.model.abc_factories import Joint, Link from adam.model.model import Model -from adam.model.abc_factories import Link, Joint def to_idyntree_solid_shape( @@ -63,7 +61,7 @@ def to_idyntree_solid_shape( def to_idyntree_link( link: Link, -) -> [idyntree.bindings.Link, List[idyntree.bindings.SolidShape]]: +) -> tuple[idyntree.bindings.Link, list[idyntree.bindings.SolidShape]]: """ Args: link (Link): the link to convert diff --git a/src/adam/model/model.py b/src/adam/model/model.py index 887b4e7a..1696edcb 100644 --- a/src/adam/model/model.py +++ b/src/adam/model/model.py @@ -1,5 +1,4 @@ import dataclasses -from typing import Dict, List from adam.model.abc_factories import Joint, Link, ModelFactory from adam.model.tree import Tree @@ -11,24 +10,29 @@ class Model: Model class. It describes the robot using links and frames and their connectivity""" name: str - links: Dict[str, Link] - frames: Dict[str, Link] - joints: Dict[str, Joint] + links: dict[str, Link] + frames: dict[str, Link] + joints: dict[str, Joint] tree: Tree NDoF: int - actuated_joints: List[str] + actuated_joints: list[str] - def __post_init__(self): - """set the "length of the model as the number of links""" - self.N = len(self.links) + @property + def N(self) -> int: + """ + + Returns: + int: the number of links in the model + """ + return len(self.links) @staticmethod - def build(factory: ModelFactory, joints_name_list: List[str] = None) -> "Model": + def build(factory: ModelFactory, joints_name_list: list[str] = None) -> "Model": """generates the model starting from the list of joints and the links-joints factory Args: factory (ModelFactory): the factory that generates the links and the joints, starting from a description (eg. urdf) - joints_name_list (List[str]): the list of the actuated joints + joints_name_list (list[str]): the list of the actuated joints Returns: Model: the model describing the robot @@ -63,9 +67,9 @@ def build(factory: ModelFactory, joints_name_list: List[str] = None) -> "Model": 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_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} + 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, @@ -77,7 +81,7 @@ def build(factory: ModelFactory, joints_name_list: List[str] = None) -> "Model": actuated_joints=joints_name_list, ) - def get_joints_chain(self, root: str, target: str) -> List[Joint]: + def get_joints_chain(self, root: str, target: str) -> list[Joint]: """generate the joints chains from a link to a link Args: @@ -85,7 +89,7 @@ def get_joints_chain(self, root: str, target: str) -> List[Joint]: target (str): the target link Returns: - List[Joint]: the list of the joints + list[Joint]: the list of the joints """ if target not in list(self.links) and target not in list(self.frames): diff --git a/src/adam/model/std_factories/std_link.py b/src/adam/model/std_factories/std_link.py index 7754747d..49475d73 100644 --- a/src/adam/model/std_factories/std_link.py +++ b/src/adam/model/std_factories/std_link.py @@ -2,7 +2,7 @@ import urdf_parser_py.urdf from adam.core.spatial_math import SpatialMath -from adam.model import Link, Inertial, Pose +from adam.model import Inertial, Link, Pose class StdLink(Link): diff --git a/src/adam/model/std_factories/std_model.py b/src/adam/model/std_factories/std_model.py index 9f3a1dcd..68337bdf 100644 --- a/src/adam/model/std_factories/std_model.py +++ b/src/adam/model/std_factories/std_model.py @@ -1,7 +1,7 @@ +import os import pathlib -from typing import List import xml.etree.ElementTree as ET -import os + import urdf_parser_py.urdf from adam.core.spatial_math import SpatialMath @@ -85,17 +85,17 @@ def __init__(self, path: str, math: SpatialMath): ) self.name = self.urdf_desc.name - def get_joints(self) -> List[StdJoint]: + def get_joints(self) -> list[StdJoint]: """ Returns: - List[StdJoint]: build the list of the joints + list[StdJoint]: build the list of the joints """ return [self.build_joint(j) for j in self.urdf_desc.joints] - def get_links(self) -> List[StdLink]: + def get_links(self) -> list[StdLink]: """ Returns: - List[StdLink]: build the list of the links + list[StdLink]: build the list of the links A link is considered a "real" link if - it has an inertial @@ -116,10 +116,10 @@ def get_links(self) -> List[StdLink]: ) ] - def get_frames(self) -> List[StdLink]: + def get_frames(self) -> list[StdLink]: """ Returns: - List[StdLink]: build the list of the links + list[StdLink]: build the list of the links A link is considered a "fake" link (frame) if - it has no inertial diff --git a/src/adam/model/tree.py b/src/adam/model/tree.py index 73d36210..e0361e21 100644 --- a/src/adam/model/tree.py +++ b/src/adam/model/tree.py @@ -1,5 +1,5 @@ import dataclasses -from typing import Dict, Iterable, List, Tuple, Union +from typing import Iterable, Iterator, Union from adam.model.abc_factories import Joint, Link @@ -10,19 +10,19 @@ class Node: name: str link: Link - arcs: List[Joint] - children: List["Node"] + arcs: list[Joint] + children: list["Node"] parent: Union[Link, None] = None parent_arc: Union[Joint, None] = None def __hash__(self) -> int: return hash(self.name) - def get_elements(self) -> Tuple[Link, Joint, Link]: + def get_elements(self) -> tuple[Link, Joint, Link]: """returns the node with its parent arc and parent link Returns: - Tuple[Link, Joint, Link]: the node, the parent_arc, the parent_link + tuple[Link, Joint, Link]: the node, the parent_arc, the parent_link """ return self.link, self.parent_arc, self.parent @@ -31,24 +31,24 @@ def get_elements(self) -> Tuple[Link, Joint, Link]: class Tree(Iterable): """The directed tree class""" - graph: Dict + graph: dict[str, Node] root: str def __post_init__(self): self.ordered_nodes_list = self.get_ordered_nodes_list(self.root) @staticmethod - def build_tree(links: List[Link], joints: List[Joint]) -> "Tree": + def build_tree(links: list[Link], joints: list[Joint]) -> "Tree": """builds the tree from the connectivity of the elements Args: - links (List[Link]) - joints (List[Joint]) + links (list[Link]) + joints (list[Joint]) 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 ) @@ -68,7 +68,9 @@ def build_tree(links: List[Link], joints: List[Joint]) -> "Tree": root_link = [l for l in nodes if nodes[l].parent is None] if len(root_link) != 1: - raise ValueError("The model has more than one root link") + raise ValueError( + f"Expected only one root, found {len(root_link)}: {root_link}" + ) return Tree(nodes, root_link[0]) def print(self, root): @@ -81,25 +83,25 @@ def print(self, root): pptree.print_tree(self.graph[root]) - def get_ordered_nodes_list(self, start: str) -> List[str]: + def get_ordered_nodes_list(self, start: str) -> list[str]: """get the ordered list of the nodes, given the connectivity Args: start (str): the start node Returns: - List[str]: the ordered list + list[str]: the ordered list """ ordered_list = [start] self.get_children(self.graph[start], ordered_list) return ordered_list @classmethod - def get_children(cls, node: Node, list: List): + def get_children(cls, node: Node, list: list): """Recursive method that finds children of child of child Args: node (Node): the analized node - list (List): the list of the children that needs to be filled + list (list): the list of the children that needs to be filled """ if node.children is not []: for child in node.children: @@ -136,7 +138,7 @@ def get_node_from_name(self, name: str) -> Node: """ return self.graph[name] - def __iter__(self) -> Node: + def __iter__(self) -> Iterator[Node]: """This method allows to iterate on the model Returns: Node: the node istance @@ -146,7 +148,7 @@ def __iter__(self) -> Node: """ yield from [self.graph[name] for name in self.ordered_nodes_list] - def __reversed__(self) -> Node: + def __reversed__(self) -> Iterator[Node]: """ Returns: Node diff --git a/src/adam/numpy/__init__.py b/src/adam/numpy/__init__.py index b94c17b8..b83df7b3 100644 --- a/src/adam/numpy/__init__.py +++ b/src/adam/numpy/__init__.py @@ -1,6 +1,4 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. from .computations import KinDynComputations from .numpy_like import NumpyLike diff --git a/src/adam/numpy/computations.py b/src/adam/numpy/computations.py index 0b72019a..9ddd2d26 100644 --- a/src/adam/numpy/computations.py +++ b/src/adam/numpy/computations.py @@ -1,6 +1,5 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. + import numpy as np @@ -17,14 +16,14 @@ def __init__( self, urdfstring: str, joints_name_list: list = None, - root_link: str = "root_link", + root_link: str = None, gravity: np.array = np.array([0, 0, -9.80665, 0, 0, 0]), ) -> None: """ Args: urdfstring (str): either path or string of the urdf joints_name_list (list): list of the actuated joints - root_link (str, optional): the first link. Defaults to 'root_link'. + root_link (str, optional): Deprecated. The root link is automatically chosen as the link with no parent in the URDF. Defaults to None. """ math = SpatialMath() factory = URDFModelFactory(path=urdfstring, math=math) @@ -32,6 +31,10 @@ def __init__( self.rbdalgos = RBDAlgorithms(model=model, math=math) self.NDoF = model.NDoF self.g = gravity + if root_link is not None: + raise DeprecationWarning( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF" + ) def set_frame_velocity_representation( self, representation: Representations diff --git a/src/adam/numpy/numpy_like.py b/src/adam/numpy/numpy_like.py index ddfac73a..41a83d29 100644 --- a/src/adam/numpy/numpy_like.py +++ b/src/adam/numpy/numpy_like.py @@ -1,6 +1,5 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. + from dataclasses import dataclass from typing import Union diff --git a/src/adam/parametric/__init__.py b/src/adam/parametric/__init__.py index 6f130a0d..9947d8db 100644 --- a/src/adam/parametric/__init__.py +++ b/src/adam/parametric/__init__.py @@ -1,3 +1 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. diff --git a/src/adam/parametric/casadi/__init__.py b/src/adam/parametric/casadi/__init__.py index 46a0a0ac..228bf6a3 100644 --- a/src/adam/parametric/casadi/__init__.py +++ b/src/adam/parametric/casadi/__init__.py @@ -1,4 +1,3 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. + from .computations_parametric import KinDynComputationsParametric diff --git a/src/adam/parametric/casadi/computations_parametric.py b/src/adam/parametric/casadi/computations_parametric.py index e9c2b77e..4149662d 100644 --- a/src/adam/parametric/casadi/computations_parametric.py +++ b/src/adam/parametric/casadi/computations_parametric.py @@ -1,7 +1,4 @@ -# 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, Union +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. import casadi as cs import numpy as np @@ -10,7 +7,7 @@ from adam.core import RBDAlgorithms from adam.core.constants import Representations from adam.model import Model -from adam.parametric.model import URDFParametricModelFactory, ParametricLink +from adam.parametric.model import ParametricLink, URDFParametricModelFactory class KinDynComputationsParametric: @@ -23,7 +20,7 @@ def __init__( urdfstring: str, joints_name_list: list, links_name_list: list, - root_link: str = "root_link", + root_link: str = None, gravity: np.array = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]), f_opts: dict = dict(jit=False, jit_options=dict(flags="-Ofast")), ) -> None: @@ -32,7 +29,7 @@ def __init__( urdfstring (str): either path or string of the urdf joints_name_list (list): list of the actuated joints links_name_list (list): list of the parametrized links - root_link (str, optional): the first link. Defaults to 'root_link'. + root_link (str, optional): Deprecated. The root link is automatically chosen as the link with no parent in the URDF. Defaults to None. """ math = SpatialMath() n_param_links = len(links_name_list) @@ -51,6 +48,10 @@ def __init__( self.NDoF = self.rbdalgos.NDoF self.g = gravity self.f_opts = f_opts + if root_link is not None: + raise DeprecationWarning( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF" + ) def set_frame_velocity_representation( self, representation: Representations @@ -261,7 +262,7 @@ def get_total_mass(self) -> cs.Function: "m", [self.length_multiplier, self.densities], [m], self.f_opts ) - def get_original_densities(self) -> List[float]: + def get_original_densities(self) -> list[float]: """Returns the original densities of the parametric links Returns: diff --git a/src/adam/parametric/jax/__init__.py b/src/adam/parametric/jax/__init__.py index 04b2fc09..ff573427 100644 --- a/src/adam/parametric/jax/__init__.py +++ b/src/adam/parametric/jax/__init__.py @@ -1,5 +1,4 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. + from .computations_parametric import KinDynComputationsParametric diff --git a/src/adam/parametric/jax/computations_parametric.py b/src/adam/parametric/jax/computations_parametric.py index 3ac9c5f2..7920006a 100644 --- a/src/adam/parametric/jax/computations_parametric.py +++ b/src/adam/parametric/jax/computations_parametric.py @@ -1,18 +1,14 @@ -# 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 +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. import jax.numpy as jnp import numpy as np from jax import grad, jit, vmap -from adam.core.rbd_algorithms import RBDAlgorithms from adam.core.constants import Representations +from adam.core.rbd_algorithms import RBDAlgorithms from adam.jax.jax_like import SpatialMath from adam.model import Model -from adam.parametric.model import URDFParametricModelFactory, ParametricLink +from adam.parametric.model import ParametricLink, URDFParametricModelFactory class KinDynComputationsParametric: @@ -25,7 +21,7 @@ def __init__( urdfstring: str, joints_name_list: list, links_name_list: list, - root_link: str = "root_link", + root_link: str = None, gravity: np.array = jnp.array([0, 0, -9.80665, 0, 0, 0]), ) -> None: """ @@ -33,7 +29,7 @@ def __init__( urdfstring (str): either path or string of the urdf joints_name_list (list): list of the actuated joints links_name_list (list): list of parametric links - root_link (str, optional): the first link. Defaults to 'root_link'. + root_link (str, optional): Deprecated. The root link is automatically chosen as the link with no parent in the URDF. Defaults to None. """ self.math = SpatialMath() self.links_name_list = links_name_list @@ -41,6 +37,10 @@ def __init__( self.urdfstring = urdfstring self.joints_name_list = joints_name_list self.representation = Representations.MIXED_REPRESENTATION # Default + if root_link is not None: + raise DeprecationWarning( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF" + ) def set_frame_velocity_representation( self, representation: Representations @@ -449,7 +449,7 @@ def get_total_mass( self.NDoF = self.rbdalgos.NDoF return self.rbdalgos.get_total_mass() - def get_original_densities(self) -> List[float]: + def get_original_densities(self) -> list[float]: """Returns the original densities of the parametric links Returns: diff --git a/src/adam/parametric/model/__init__.py b/src/adam/parametric/model/__init__.py index 176c2349..87813a22 100644 --- a/src/adam/parametric/model/__init__.py +++ b/src/adam/parametric/model/__init__.py @@ -1,6 +1,5 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. + from .parametric_factories.parametric_joint import ParametricJoint from .parametric_factories.parametric_link import ParametricLink diff --git a/src/adam/parametric/model/parametric_factories/__init__.py b/src/adam/parametric/model/parametric_factories/__init__.py index 6f130a0d..9947d8db 100644 --- a/src/adam/parametric/model/parametric_factories/__init__.py +++ b/src/adam/parametric/model/parametric_factories/__init__.py @@ -1,3 +1 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. diff --git a/src/adam/parametric/model/parametric_factories/parametric_link.py b/src/adam/parametric/model/parametric_factories/parametric_link.py index 1346f3f8..82d2c571 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_link.py +++ b/src/adam/parametric/model/parametric_factories/parametric_link.py @@ -1,12 +1,12 @@ +import copy +import math +from enum import Enum + 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 - -import math from adam.model.abc_factories import Inertia, Inertial diff --git a/src/adam/parametric/model/parametric_factories/parametric_model.py b/src/adam/parametric/model/parametric_factories/parametric_model.py index 3702735e..8d38b226 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_model.py +++ b/src/adam/parametric/model/parametric_factories/parametric_model.py @@ -1,11 +1,8 @@ -import pathlib -from typing import List -import os - 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, get_xml_string +from adam.model import Joint, Link, ModelFactory, StdJoint, StdLink +from adam.model.std_factories.std_model import get_xml_string, urdf_remove_sensors_tags from adam.parametric.model import ParametricJoint, ParametricLink @@ -21,7 +18,7 @@ def __init__( self, path: str, math: SpatialMath, - links_name_list: List, + links_name_list: list, length_multiplier, densities, ): @@ -45,26 +42,26 @@ def __init__( self.length_multiplier = length_multiplier self.densities = densities - def get_joints(self) -> List[Joint]: + def get_joints(self) -> list[Joint]: """ Returns: - List[Joint]: build the list of the joints + list[Joint]: build the list of the joints """ return [self.build_joint(j) for j in self.urdf_desc.joints] - def get_links(self) -> List[Link]: + def get_links(self) -> list[Link]: """ Returns: - List[Link]: build the list of the links + list[Link]: build the list of the links """ return [ self.build_link(l) for l in self.urdf_desc.links if l.inertial is not None ] - def get_frames(self) -> List[StdLink]: + def get_frames(self) -> list[StdLink]: """ Returns: - List[Link]: build the list of the links + list[Link]: build the list of the links """ return [self.build_link(l) for l in self.urdf_desc.links if l.inertial is None] diff --git a/src/adam/parametric/numpy/__init__.py b/src/adam/parametric/numpy/__init__.py index 04b2fc09..ff573427 100644 --- a/src/adam/parametric/numpy/__init__.py +++ b/src/adam/parametric/numpy/__init__.py @@ -1,5 +1,4 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. + from .computations_parametric import KinDynComputationsParametric diff --git a/src/adam/parametric/numpy/computations_parametric.py b/src/adam/parametric/numpy/computations_parametric.py index 7e961467..f7030376 100644 --- a/src/adam/parametric/numpy/computations_parametric.py +++ b/src/adam/parametric/numpy/computations_parametric.py @@ -1,15 +1,12 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. import numpy as np -from typing import List -from adam.core.rbd_algorithms import RBDAlgorithms from adam.core.constants import Representations +from adam.core.rbd_algorithms import RBDAlgorithms from adam.model import Model -from adam.parametric.model import URDFParametricModelFactory, ParametricLink from adam.numpy.numpy_like import SpatialMath +from adam.parametric.model import ParametricLink, URDFParametricModelFactory class KinDynComputationsParametric: @@ -22,7 +19,7 @@ def __init__( urdfstring: str, joints_name_list: list, links_name_list: list, - root_link: str = "root_link", + root_link: str = None, gravity: np.array = np.array([0, 0, -9.80665, 0, 0, 0]), ) -> None: """ @@ -30,7 +27,7 @@ def __init__( urdfstring (str): either path or string of the urdf joints_name_list (list): list of the actuated joints links_name_list (list): list of parametric links - root_link (str, optional): the first link. Defaults to 'root_link'. + root_link (str, optional): Deprecated. The root link is automatically chosen as the link with no parent in the URDF. Defaults to None. """ self.links_name_list = links_name_list self.math = SpatialMath() @@ -38,6 +35,10 @@ def __init__( self.urdfstring = urdfstring self.joints_name_list = joints_name_list self.representation = Representations.MIXED_REPRESENTATION # Default + if root_link is not None: + raise DeprecationWarning( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF" + ) def set_frame_velocity_representation( self, representation: Representations @@ -443,7 +444,7 @@ def get_total_mass( self.NDoF = model.NDoF return self.rbdalgos.get_total_mass() - def get_original_densities(self) -> List[float]: + def get_original_densities(self) -> list[float]: """Returns the original densities of the parametric links Returns: diff --git a/src/adam/parametric/pytorch/__init__.py b/src/adam/parametric/pytorch/__init__.py index 04b2fc09..ff573427 100644 --- a/src/adam/parametric/pytorch/__init__.py +++ b/src/adam/parametric/pytorch/__init__.py @@ -1,5 +1,4 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. + from .computations_parametric import KinDynComputationsParametric diff --git a/src/adam/parametric/pytorch/computations_parametric.py b/src/adam/parametric/pytorch/computations_parametric.py index 07eb7a78..63568d45 100644 --- a/src/adam/parametric/pytorch/computations_parametric.py +++ b/src/adam/parametric/pytorch/computations_parametric.py @@ -1,15 +1,12 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. 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.core.rbd_algorithms import RBDAlgorithms from adam.model import Model -from adam.parametric.model import URDFParametricModelFactory, ParametricLink +from adam.parametric.model import ParametricLink, URDFParametricModelFactory from adam.pytorch.torch_like import SpatialMath @@ -23,7 +20,7 @@ def __init__( urdfstring: str, joints_name_list: list, links_name_list: list, - root_link: str = "root_link", + root_link: str = None, gravity: np.array = torch.tensor( [0, 0, -9.80665, 0, 0, 0], dtype=torch.float64 ), @@ -33,7 +30,7 @@ def __init__( urdfstring (str): either path or string of the urdf joints_name_list (list): list of the actuated joints links_name_list (list): list of parametric links - root_link (str, optional): the first link. Defaults to 'root_link'. + root_link (str, optional): Deprecated. The root link is automatically chosen as the link with no parent in the URDF. Defaults to None. """ self.math = SpatialMath() self.g = gravity @@ -41,6 +38,10 @@ def __init__( self.joints_name_list = joints_name_list self.urdfstring = urdfstring self.representation = Representations.MIXED_REPRESENTATION # Default + if root_link is not None: + raise DeprecationWarning( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF" + ) def set_frame_velocity_representation( self, representation: Representations @@ -447,7 +448,7 @@ def get_total_mass( self.NDoF = self.rbdalgos.NDoF return self.rbdalgos.get_total_mass() - def get_original_densities(self) -> List[float]: + def get_original_densities(self) -> list[float]: """Returns the original densities of the parametric links Returns: diff --git a/src/adam/pytorch/__init__.py b/src/adam/pytorch/__init__.py index 7a33bea1..b427f55a 100644 --- a/src/adam/pytorch/__init__.py +++ b/src/adam/pytorch/__init__.py @@ -1,7 +1,6 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. + -from .computations import KinDynComputations from .computation_batch import KinDynComputationsBatch +from .computations import KinDynComputations from .torch_like import TorchLike diff --git a/src/adam/pytorch/computations.py b/src/adam/pytorch/computations.py index 060a2075..3fc70805 100644 --- a/src/adam/pytorch/computations.py +++ b/src/adam/pytorch/computations.py @@ -1,6 +1,5 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. + import numpy as np import torch @@ -18,7 +17,7 @@ def __init__( self, urdfstring: str, joints_name_list: list = None, - root_link: str = "root_link", + root_link: str = None, gravity: np.array = torch.tensor( [0, 0, -9.80665, 0, 0, 0], dtype=torch.float64 ), @@ -27,7 +26,7 @@ def __init__( Args: urdfstring (str): either path or string of the urdf joints_name_list (list): list of the actuated joints - root_link (str, optional): the first link. Defaults to 'root_link'. + root_link (str, optional): Deprecated. The root link is automatically chosen as the link with no parent in the URDF. Defaults to None. """ math = SpatialMath() factory = URDFModelFactory(path=urdfstring, math=math) @@ -35,6 +34,10 @@ def __init__( self.rbdalgos = RBDAlgorithms(model=model, math=math) self.NDoF = self.rbdalgos.NDoF self.g = gravity + if root_link is not None: + raise DeprecationWarning( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF" + ) def set_frame_velocity_representation( self, representation: Representations diff --git a/src/adam/pytorch/torch_like.py b/src/adam/pytorch/torch_like.py index 71962f65..de2cb812 100644 --- a/src/adam/pytorch/torch_like.py +++ b/src/adam/pytorch/torch_like.py @@ -1,13 +1,11 @@ -# 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. +# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. from dataclasses import dataclass from typing import Union +import numpy as np import numpy.typing as ntp import torch -import numpy as np from adam.core.spatial_math import ArrayLike, ArrayLikeFactory, SpatialMath diff --git a/tests/conftest.py b/tests/conftest.py index 0e9b491f..31c582bf 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -1,15 +1,17 @@ -import pytest -import numpy as np -import icub_models -import idyntree.bindings as idyntree -from adam import Representations -from adam.numpy.numpy_like import SpatialMath import dataclasses -from itertools import product import logging import os +from itertools import product + +import icub_models +import idyntree.bindings as idyntree +import numpy as np +import pytest import requests +from adam import Representations +from adam.numpy.numpy_like import SpatialMath + @dataclasses.dataclass class State: diff --git a/tests/parametric/test_casadi_parametric.py b/tests/parametric/test_casadi_parametric.py index 43dbae84..b8aa4cad 100644 --- a/tests/parametric/test_casadi_parametric.py +++ b/tests/parametric/test_casadi_parametric.py @@ -1,8 +1,9 @@ import casadi as cs import numpy as np import pytest +from conftest import RobotCfg, State + from adam.parametric.casadi import KinDynComputationsParametric -from conftest import State, RobotCfg @pytest.fixture(scope="module") diff --git a/tests/parametric/test_idyntree_conversion_parametric.py b/tests/parametric/test_idyntree_conversion_parametric.py index 29c8f70d..3d7fc4ea 100644 --- a/tests/parametric/test_idyntree_conversion_parametric.py +++ b/tests/parametric/test_idyntree_conversion_parametric.py @@ -1,14 +1,15 @@ import casadi as cs import numpy as np import pytest -from adam.parametric.casadi import KinDynComputationsParametric -from conftest import State, RobotCfg, compute_idyntree_values +from conftest import RobotCfg, State, compute_idyntree_values + +from adam.model import Model from adam.model.conversions.idyntree import to_idyntree_model +from adam.numpy.numpy_like import SpatialMath +from adam.parametric.casadi import KinDynComputationsParametric from adam.parametric.model.parametric_factories.parametric_model import ( URDFParametricModelFactory, ) -from adam.model import Model -from adam.numpy.numpy_like import SpatialMath @pytest.fixture(scope="module") diff --git a/tests/parametric/test_jax_parametric.py b/tests/parametric/test_jax_parametric.py index f1aed86f..75780bc2 100644 --- a/tests/parametric/test_jax_parametric.py +++ b/tests/parametric/test_jax_parametric.py @@ -1,7 +1,8 @@ import numpy as np import pytest +from conftest import RobotCfg, State + from adam.parametric.jax import KinDynComputationsParametric -from conftest import State, RobotCfg @pytest.fixture(scope="module") diff --git a/tests/parametric/test_numpy_parametric.py b/tests/parametric/test_numpy_parametric.py index 54e2c47f..15e4ec2f 100644 --- a/tests/parametric/test_numpy_parametric.py +++ b/tests/parametric/test_numpy_parametric.py @@ -1,7 +1,8 @@ import numpy as np import pytest +from conftest import RobotCfg, State + from adam.parametric.numpy import KinDynComputationsParametric -from conftest import State, RobotCfg @pytest.fixture(scope="module") diff --git a/tests/parametric/test_pytorch_parametric.py b/tests/parametric/test_pytorch_parametric.py index 9ec765b7..840007c4 100644 --- a/tests/parametric/test_pytorch_parametric.py +++ b/tests/parametric/test_pytorch_parametric.py @@ -1,8 +1,9 @@ import numpy as np import pytest -from adam.parametric.pytorch import KinDynComputationsParametric -from conftest import State, RobotCfg import torch +from conftest import RobotCfg, State + +from adam.parametric.pytorch import KinDynComputationsParametric torch.set_default_dtype(torch.float64) diff --git a/tests/test_casadi.py b/tests/test_casadi.py index 466c16f9..4f213a4d 100644 --- a/tests/test_casadi.py +++ b/tests/test_casadi.py @@ -1,8 +1,9 @@ import casadi as cs import numpy as np import pytest +from conftest import RobotCfg, State + from adam.casadi import KinDynComputations -from conftest import State, RobotCfg @pytest.fixture(scope="module") diff --git a/tests/test_idyntree_conversion.py b/tests/test_idyntree_conversion.py index ea4bc1fe..ceb5f779 100644 --- a/tests/test_idyntree_conversion.py +++ b/tests/test_idyntree_conversion.py @@ -1,8 +1,9 @@ import casadi as cs import numpy as np import pytest +from conftest import RobotCfg, State, compute_idyntree_values + from adam.casadi import KinDynComputations -from conftest import State, RobotCfg, compute_idyntree_values from adam.model.conversions.idyntree import to_idyntree_model diff --git a/tests/test_jax.py b/tests/test_jax.py index 94c788e9..a74449e6 100644 --- a/tests/test_jax.py +++ b/tests/test_jax.py @@ -1,9 +1,10 @@ import numpy as np import pytest -from adam.jax import KinDynComputations -from conftest import State, RobotCfg +from conftest import RobotCfg, State from jax import config +from adam.jax import KinDynComputations + config.update("jax_enable_x64", True) diff --git a/tests/test_numpy.py b/tests/test_numpy.py index f5619ed2..ba3e608c 100644 --- a/tests/test_numpy.py +++ b/tests/test_numpy.py @@ -1,7 +1,8 @@ import numpy as np import pytest +from conftest import RobotCfg, State + from adam.numpy import KinDynComputations -from conftest import State, RobotCfg @pytest.fixture(scope="module") diff --git a/tests/test_pytorch.py b/tests/test_pytorch.py index a85c8290..2af3bbc4 100644 --- a/tests/test_pytorch.py +++ b/tests/test_pytorch.py @@ -1,8 +1,9 @@ import numpy as np import pytest -from adam.pytorch import KinDynComputations -from conftest import State, RobotCfg import torch +from conftest import RobotCfg, State + +from adam.pytorch import KinDynComputations torch.set_default_dtype(torch.float64) diff --git a/tests/test_pytorch_batch.py b/tests/test_pytorch_batch.py index 51435cb7..4839b207 100644 --- a/tests/test_pytorch_batch.py +++ b/tests/test_pytorch_batch.py @@ -1,10 +1,11 @@ import numpy as np import pytest -from adam.pytorch import KinDynComputationsBatch -from conftest import State, RobotCfg import torch +from conftest import RobotCfg, State from jax import config +from adam.pytorch import KinDynComputationsBatch + config.update("jax_enable_x64", True)