Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix comments and function arguments #116

Merged
merged 8 commits into from
Jan 8, 2025
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 9 additions & 5 deletions src/adam/casadi/computations.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved.

import warnings

import casadi as cs
import numpy as np

Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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:
Expand Down
8 changes: 3 additions & 5 deletions src/adam/core/rbd_algorithms.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
"""
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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

Expand Down
8 changes: 6 additions & 2 deletions src/adam/jax/computations.py
Original file line number Diff line number Diff line change
@@ -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

Expand Down Expand Up @@ -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(
Expand Down
7 changes: 5 additions & 2 deletions src/adam/numpy/computations.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved.

import warnings

import numpy as np

Expand Down Expand Up @@ -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(
Expand Down
8 changes: 6 additions & 2 deletions src/adam/parametric/casadi/computations_parametric.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved.

import warnings

import casadi as cs
import numpy as np

Expand Down Expand Up @@ -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(
Expand Down
8 changes: 6 additions & 2 deletions src/adam/parametric/jax/computations_parametric.py
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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(
Expand Down
8 changes: 6 additions & 2 deletions src/adam/parametric/numpy/computations_parametric.py
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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(
Expand Down
8 changes: 6 additions & 2 deletions src/adam/parametric/pytorch/computations_parametric.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved.

import warnings

import numpy as np
import torch

Expand Down Expand Up @@ -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(
Expand Down
12 changes: 10 additions & 2 deletions src/adam/pytorch/computation_batch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)
Expand All @@ -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
Expand Down
26 changes: 15 additions & 11 deletions src/adam/pytorch/computations.py
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
# Copyright (C) Istituto Italiano di Tecnologia (IIT). All rights reserved.


import warnings

import numpy as np
import torch

Expand Down Expand Up @@ -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(
Expand All @@ -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

Expand All @@ -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

Expand All @@ -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

Expand All @@ -96,7 +100,7 @@ def forward_kinematics(
self.rbdalgos.forward_kinematics(
frame,
base_transform,
joint_position,
joint_positions,
)
).array

Expand Down Expand Up @@ -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:
Expand All @@ -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:
Expand All @@ -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:
Expand Down
Loading