diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index ff67fda2..1d7c2e9d 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -1,5 +1,7 @@ # Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. +import warnings + import casadi as cs import numpy as np @@ -34,8 +36,10 @@ def __init__( 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" + warnings.warn( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF", + DeprecationWarning, + stacklevel=2, ) def set_frame_velocity_representation( @@ -294,7 +298,7 @@ def jacobian_dot( frame (str): The frame to which the jacobian will be computed 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 + base_velocity (cs.SX): The base velocity joint_velocities (cs.SX): The joint velocities Returns: @@ -369,7 +373,7 @@ def bias_force( Args: 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 + base_velocity (cs.SX): The base velocity joint_velocities (cs.SX): The joints velocity Returns: @@ -402,7 +406,7 @@ def coriolis_term( Args: 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 + base_velocity (cs.SX): The base velocity joint_velocities (cs.SX): The joints velocity Returns: diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 4c67d980..de2f744d 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -8,9 +8,7 @@ class RBDAlgorithms: - """This is a small class that implements Rigid body algorithms retrieving robot quantities represented - in mixed representation, for Floating Base systems - as humanoid robots. - """ + """This is a small class that implements Rigid body algorithms retrieving robot quantities, for Floating Base systems - as humanoid robots.""" def __init__(self, model: Model, math: SpatialMath) -> None: """ @@ -248,7 +246,7 @@ def jacobian_dot( frame (str): The frame to which the jacobian will be computed base_transform (npt.ArrayLike): The homogenous transform from base to world frame joint_positions (npt.ArrayLike): The joints position - base_velocity (npt.ArrayLike): The base velocity in mixed representation + base_velocity (npt.ArrayLike): The base velocity joint_velocities (npt.ArrayLike): The joints velocity Returns: @@ -370,7 +368,7 @@ def rnea( frame (str): The frame to which the jacobian will be computed base_transform (T): The homogenous transform from base to world frame joint_positions (T): The joints position - base_velocity (T): The base velocity in mixed representation + base_velocity (T): The base velocity joint_velocities (T): The joints velocity g (T): The 6D gravity acceleration diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index 82d3611f..7578ba17 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -1,5 +1,7 @@ # Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. +import warnings + import jax.numpy as jnp import numpy as np @@ -32,8 +34,10 @@ def __init__( 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" + warnings.warn( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF", + DeprecationWarning, + stacklevel=2, ) def set_frame_velocity_representation( diff --git a/src/adam/numpy/computations.py b/src/adam/numpy/computations.py index 9ddd2d26..249a891d 100644 --- a/src/adam/numpy/computations.py +++ b/src/adam/numpy/computations.py @@ -1,5 +1,6 @@ # Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. +import warnings import numpy as np @@ -32,8 +33,10 @@ def __init__( 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" + warnings.warn( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF", + DeprecationWarning, + stacklevel=2, ) def set_frame_velocity_representation( diff --git a/src/adam/parametric/casadi/computations_parametric.py b/src/adam/parametric/casadi/computations_parametric.py index 4149662d..6fc8cd3b 100644 --- a/src/adam/parametric/casadi/computations_parametric.py +++ b/src/adam/parametric/casadi/computations_parametric.py @@ -1,5 +1,7 @@ # Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. +import warnings + import casadi as cs import numpy as np @@ -49,8 +51,10 @@ def __init__( 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" + warnings.warn( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF", + DeprecationWarning, + stacklevel=2, ) def set_frame_velocity_representation( diff --git a/src/adam/parametric/jax/computations_parametric.py b/src/adam/parametric/jax/computations_parametric.py index 7920006a..2fa061de 100644 --- a/src/adam/parametric/jax/computations_parametric.py +++ b/src/adam/parametric/jax/computations_parametric.py @@ -1,5 +1,7 @@ # Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. +import warnings + import jax.numpy as jnp import numpy as np from jax import grad, jit, vmap @@ -38,8 +40,10 @@ def __init__( 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" + warnings.warn( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF", + DeprecationWarning, + stacklevel=2, ) def set_frame_velocity_representation( diff --git a/src/adam/parametric/numpy/computations_parametric.py b/src/adam/parametric/numpy/computations_parametric.py index f7030376..bcea1e32 100644 --- a/src/adam/parametric/numpy/computations_parametric.py +++ b/src/adam/parametric/numpy/computations_parametric.py @@ -1,5 +1,7 @@ # Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. +import warnings + import numpy as np from adam.core.constants import Representations @@ -36,8 +38,10 @@ def __init__( 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" + warnings.warn( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF", + DeprecationWarning, + stacklevel=2, ) def set_frame_velocity_representation( diff --git a/src/adam/parametric/pytorch/computations_parametric.py b/src/adam/parametric/pytorch/computations_parametric.py index 63568d45..a185ee02 100644 --- a/src/adam/parametric/pytorch/computations_parametric.py +++ b/src/adam/parametric/pytorch/computations_parametric.py @@ -1,5 +1,7 @@ # Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. +import warnings + import numpy as np import torch @@ -39,8 +41,10 @@ def __init__( 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" + warnings.warn( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF", + DeprecationWarning, + stacklevel=2, ) def set_frame_velocity_representation( diff --git a/src/adam/pytorch/computation_batch.py b/src/adam/pytorch/computation_batch.py index b5e96a10..6ea59fdc 100644 --- a/src/adam/pytorch/computation_batch.py +++ b/src/adam/pytorch/computation_batch.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. +import warnings + import jax import jax.numpy as jnp import numpy as np @@ -23,14 +25,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): path 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) @@ -39,6 +41,12 @@ def __init__( self.NDoF = self.rbdalgos.NDoF self.g = gravity self.funcs = {} + if root_link is not None: + warnings.warn( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF", + DeprecationWarning, + stacklevel=2, + ) def set_frame_velocity_representation( self, representation: Representations diff --git a/src/adam/pytorch/computations.py b/src/adam/pytorch/computations.py index 3fc70805..54fb02d2 100644 --- a/src/adam/pytorch/computations.py +++ b/src/adam/pytorch/computations.py @@ -1,6 +1,8 @@ # Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved. +import warnings + import numpy as np import torch @@ -35,8 +37,10 @@ def __init__( 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" + warnings.warn( + "The root_link argument is not used. The root link is automatically chosen as the link with no parent in the URDF", + DeprecationWarning, + stacklevel=2, ) def set_frame_velocity_representation( @@ -50,7 +54,7 @@ def set_frame_velocity_representation( self.rbdalgos.set_frame_velocity_representation(representation) def mass_matrix( - self, base_transform: torch.Tensor, joint_position: torch.Tensor + self, base_transform: torch.Tensor, joint_positions: torch.Tensor ) -> torch.Tensor: """Returns the Mass Matrix functions computed the CRBA @@ -61,11 +65,11 @@ def mass_matrix( Returns: M (torch.tensor): Mass Matrix """ - [M, _] = self.rbdalgos.crba(base_transform, joint_position) + [M, _] = self.rbdalgos.crba(base_transform, joint_positions) return M.array def centroidal_momentum_matrix( - self, base_transform: torch.Tensor, joint_position: torch.Tensor + self, base_transform: torch.Tensor, joint_positions: torch.Tensor ) -> torch.Tensor: """Returns the Centroidal Momentum Matrix functions computed the CRBA @@ -76,11 +80,11 @@ def centroidal_momentum_matrix( Returns: Jcc (torch.tensor): Centroidal Momentum matrix """ - [_, Jcm] = self.rbdalgos.crba(base_transform, joint_position) + [_, Jcm] = self.rbdalgos.crba(base_transform, joint_positions) return Jcm.array def forward_kinematics( - self, frame, base_transform: torch.Tensor, joint_position: torch.Tensor + self, frame, base_transform: torch.Tensor, joint_positions: torch.Tensor ) -> torch.Tensor: """Computes the forward kinematics relative to the specified frame @@ -96,7 +100,7 @@ def forward_kinematics( self.rbdalgos.forward_kinematics( frame, base_transform, - joint_position, + joint_positions, ) ).array @@ -177,7 +181,7 @@ def bias_force( base_velocity: torch.Tensor, joint_velocities: torch.Tensor, ) -> torch.Tensor: - """Returns the bias force of the floating-base dynamics ejoint_positionsuation, + """Returns the bias force of the floating-base dynamics equation, using a reduced RNEA (no acceleration and external forces) Args: @@ -204,7 +208,7 @@ def coriolis_term( base_velocity: torch.Tensor, joint_velocities: torch.Tensor, ) -> torch.Tensor: - """Returns the coriolis term of the floating-base dynamics ejoint_positionsuation, + """Returns the coriolis term of the floating-base dynamics equation, using a reduced RNEA (no acceleration and external forces) Args: @@ -228,7 +232,7 @@ def coriolis_term( def gravity_term( self, base_transform: torch.Tensor, joint_positions: torch.Tensor ) -> torch.Tensor: - """Returns the gravity term of the floating-base dynamics ejoint_positionsuation, + """Returns the gravity term of the floating-base dynamics equation, using a reduced RNEA (no acceleration and external forces) Args: