From 379522d2e2b6b7e13abee4cee1ed939607eb6e10 Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 4 Aug 2023 11:33:23 +0200 Subject: [PATCH 01/39] Manage imports --- src/adam/__init__.py | 2 ++ src/adam/casadi/__init__.py | 2 +- src/adam/core/__init__.py | 1 + 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/adam/__init__.py b/src/adam/__init__.py index 6f130a0d..6d51bece 100644 --- a/src/adam/__init__.py +++ b/src/adam/__init__.py @@ -1,3 +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. + +from adam.core import Representations diff --git a/src/adam/casadi/__init__.py b/src/adam/casadi/__init__.py index c3bc4744..1c3b0795 100644 --- a/src/adam/casadi/__init__.py +++ b/src/adam/casadi/__init__.py @@ -2,5 +2,5 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -from .computations import KinDynComputations from .casadi_like import CasadiLike +from .computations import KinDynComputations diff --git a/src/adam/core/__init__.py b/src/adam/core/__init__.py index a5a25cd9..b8a1addc 100644 --- a/src/adam/core/__init__.py +++ b/src/adam/core/__init__.py @@ -2,5 +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 .constants import Representations from .rbd_algorithms import RBDAlgorithms from .spatial_math import SpatialMath From c808f418cb07bbfc6b336b3ab0d5ed918ae5149f Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 4 Aug 2023 11:33:46 +0200 Subject: [PATCH 02/39] Add velocity representation enum --- src/adam/core/constants.py | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 src/adam/core/constants.py diff --git a/src/adam/core/constants.py b/src/adam/core/constants.py new file mode 100644 index 00000000..fe4e4614 --- /dev/null +++ b/src/adam/core/constants.py @@ -0,0 +1,6 @@ +from enum import Enum + + +class Representations(Enum): + BODY_FIXED_REPRESENTATION = 0 + MIXED_REPRESENTATION = 1 From 6a36b22bec26d40af5790fb69adb82d9caf5a978 Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 4 Aug 2023 11:34:45 +0200 Subject: [PATCH 03/39] Adapt crba --- src/adam/core/rbd_algorithms.py | 22 ++++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 520533f4..7e4ef7e7 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -103,14 +103,20 @@ def crba( elif joint_i.idx is not None: Jcm[:, joint_i.idx + 6] = X_G[i].T @ Ic[i] @ Phi[i] - # Until now the algorithm returns the joint_position quantities in Body Fixed representation - # Moving to mixed representation... - X_to_mixed = self.math.factory.eye(self.model.NDoF + 6) - X_to_mixed[:3, :3] = base_transform[:3, :3].T - X_to_mixed[3:6, 3:6] = base_transform[:3, :3].T - M = X_to_mixed.T @ M @ X_to_mixed - Jcm = X_to_mixed[:6, :6].T @ Jcm @ X_to_mixed - return M, Jcm + if ( + self.frame_velocity_representation + == Representations.BODY_FIXED_REPRESENTATION + ): + return M, Jcm + + if self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: + # Until now the algorithm returns the joint_position quantities in Body Fixed representation + # Moving to mixed representation... + X_to_mixed = self.math.factory.eye(self.model.NDoF + 6) + X_to_mixed[:6, :6] = self.math.adjoint_mixed_inverse(base_transform) + M = X_to_mixed.T @ M @ X_to_mixed + Jcm = X_to_mixed[:6, :6].T @ Jcm @ X_to_mixed + return M, Jcm def forward_kinematics( self, frame, base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike From bbcdf3a2b21353ad1b141fab6ee6fd47fbfaab13 Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 4 Aug 2023 11:35:05 +0200 Subject: [PATCH 04/39] Adapt Jacobian --- src/adam/core/rbd_algorithms.py | 37 +++++++++++++++++++++++++-------- 1 file changed, 28 insertions(+), 9 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 7e4ef7e7..8eecbcc7 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -179,16 +179,35 @@ def joints_jacobian( def jacobian( self, frame: str, base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike ) -> npt.ArrayLike: - J = self.joints_jacobian(frame, base_transform, joint_positions) - T_ee = self.forward_kinematics(frame, base_transform, joint_positions) - # Adding the floating base part of the Jacobian, in Mixed representation + eye = self.math.factory.eye(4) + J = self.joints_jacobian(frame, eye, joint_positions) + T_ee = self.forward_kinematics(frame, eye, joint_positions) + X = self.math.factory.zeros(6, 6) + L_X_B = self.math.adjoint_inverse(T_ee) + J = ( + self.math.adjoint_mixed_inverse(T_ee) @ J + ) # J for now is B_J_L, we need L_J_L, so we need to multiply by L_R_B J_tot = self.math.factory.zeros(6, self.NDoF + 6) - J_tot[:3, :3] = self.math.factory.eye(3) - J_tot[:3, 3:6] = -self.math.skew((T_ee[:3, 3] - base_transform[:3, 3])) - J_tot[:3, 6:] = J[:3, :] - J_tot[3:, 3:6] = self.math.factory.eye(3) - J_tot[3:, 6:] = J[3:, :] - return J_tot + J_tot[:6, :6] = L_X_B + J_tot[:, 6:] = J + + if ( + self.frame_velocity_representation + == Representations.BODY_FIXED_REPRESENTATION + ): + return J_tot + # let's move to mixed representation + elif self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: + w_H_L = base_transform @ T_ee.array + LI_X_L = self.math.adjoint_mixed(w_H_L) + X = self.math.factory.eye(6 + self.NDoF) + X[:6, :6] = self.math.adjoint_mixed_inverse(base_transform) + J_tot = LI_X_L @ J_tot @ X + return J_tot + else: + raise NotImplementedError( + "Only BODY_FIXED_REPRESENTATION and MIXED_REPRESENTATION are implemented" + ) def relative_jacobian( self, frame: str, joint_positions: npt.ArrayLike From 6e524150df500721233aa5f48c9c7bd74ed6ad57 Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 4 Aug 2023 11:35:49 +0200 Subject: [PATCH 05/39] Adapt relative jacobian --- src/adam/core/rbd_algorithms.py | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 8eecbcc7..9e42b243 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -221,8 +221,22 @@ def relative_jacobian( Returns: J (npt.ArrayLike): The Jacobian between the root and the frame """ - base_transform = self.math.factory.eye(4) - return self.joints_jacobian(frame, base_transform, joint_positions) + eye = self.math.factory.eye(4) + T_ee = self.forward_kinematics(frame, eye, joint_positions) + if self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: + return self.joints_jacobian(frame, eye, joint_positions) + + elif ( + self.frame_velocity_representation + == Representations.BODY_FIXED_REPRESENTATION + ): + return self.math.adjoint_mixed_inverse(T_ee) @ self.joints_jacobian( + frame, eye, joint_positions + ) # J for now is B_J_L, we need L_J_L, so we need to multiply by L_R_B + else: + raise NotImplementedError( + "Only BODY_FIXED_REPRESENTATION and MIXED_REPRESENTATION are implemented" + ) def CoM_position( self, base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike From 76bcf05335a619c4cdc6d5eb2783da4fd1c4f851 Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 4 Aug 2023 11:36:10 +0200 Subject: [PATCH 06/39] Adapt rnea --- src/adam/core/rbd_algorithms.py | 39 ++++++++++++++++++++++----------- 1 file changed, 26 insertions(+), 13 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 9e42b243..80c80bf4 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -302,20 +302,33 @@ def rnea( a = [None] * model_len f = [None] * model_len - X_to_mixed = self.math.adjoint(base_transform[:3, :3]) - - acc_to_mixed = self.math.factory.zeros(6, 1) - acc_to_mixed[:3] = ( - -X_to_mixed[:3, :3] @ self.math.skew(base_velocity[3:]) @ base_velocity[:3] - ) - acc_to_mixed[3:] = ( - -X_to_mixed[:3, :3] @ self.math.skew(base_velocity[3:]) @ base_velocity[3:] - ) + transformed_acceleration = self.math.factory.zeros(6, 1) + gravity_transform = self.math.adjoint_mixed_inverse(base_transform) + if ( + self.frame_velocity_representation + == Representations.BODY_FIXED_REPRESENTATION + ): + B_X_BI = self.math.factory.eye(6) + + elif self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: + B_X_BI = self.math.adjoint_mixed_inverse(base_transform) + transformed_acceleration[:3] = ( + -B_X_BI[:3, :3] @ self.math.skew(base_velocity[3:]) @ base_velocity[:3] + ) + # transformed_acceleration[3:] = ( + # -X_to_mixed[:3, :3] + # @ self.math.skew(base_velocity[3:]) + # @ base_velocity[3:] + # ) + else: + raise NotImplementedError( + "Only BODY_FIXED_REPRESENTATION and MIXED_REPRESENTATION are implemented" + ) + # set initial acceleration (rotated gravity + apparent acceleration) # reshape g as a vertical vector - a[0] = -X_to_mixed @ g.reshape(-1, 1) + acc_to_mixed + a[0] = -gravity_transform @ g.reshape(6, 1) + transformed_acceleration - # for i in range(self.tree.N): for i, node in enumerate(self.model.tree): node: Node link_i, joint_i, link_pi = node.get_elements() @@ -326,7 +339,7 @@ def rnea( self.math.factory.eye(3), self.math.factory.zeros(3, 1) ) Phi[i] = self.math.factory.eye(6) - v[i] = X_to_mixed @ base_velocity + v[i] = B_X_BI @ base_velocity a[i] = X_p[i] @ a[0] else: q = joint_positions[joint_i.idx] if joint_i.idx is not None else 0.0 @@ -353,7 +366,7 @@ def rnea( pi = self.model.tree.get_idx_from_name(link_pi.name) f[pi] = f[pi] + X_p[i].T @ f[i] - tau[:6] = X_to_mixed.T @ tau[:6] + tau[:6] = B_X_BI.T @ tau[:6] return tau def aba(self): From e1d2fc0a9f0efa207fb06d653ab97dcaff2ca3ff Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 4 Aug 2023 11:36:31 +0200 Subject: [PATCH 07/39] Set representation --- src/adam/core/rbd_algorithms.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 80c80bf4..4039022c 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -3,8 +3,9 @@ # GNU Lesser General Public License v2.1 or any later version. import numpy.typing as npt -from adam.model import Model, Node +from adam.core.constants import Representations from adam.core.spatial_math import SpatialMath +from adam.model import Model, Node class RBDAlgorithms: @@ -24,6 +25,17 @@ def __init__(self, model: Model, math: SpatialMath) -> None: self.NDoF = model.NDoF self.root_link = self.model.tree.root self.math = math + self.frame_velocity_representation = ( + Representations.MIXED_REPRESENTATION + ) # default + + def set_frame_velocity_representation(self, representation: Representations): + """Sets the frame velocity representation + + Args: + representation (str): The representation of the frame velocity + """ + self.frame_velocity_representation = representation def crba( self, base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike From 02a4e5ec83c7da68424336d9a3ba65f1cabd704a Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 4 Aug 2023 11:37:39 +0200 Subject: [PATCH 08/39] Add right adjoint transforms --- src/adam/core/spatial_math.py | 48 +++++++++++++++++++++++++++++++++-- 1 file changed, 46 insertions(+), 2 deletions(-) diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py index 296ab0dd..6dbf6a10 100644 --- a/src/adam/core/spatial_math.py +++ b/src/adam/core/spatial_math.py @@ -404,13 +404,57 @@ def spatial_skew_star(self, v: npt.ArrayLike) -> npt.ArrayLike: """ return -self.spatial_skew(v).T - def adjoint(self, R: npt.ArrayLike) -> npt.ArrayLike: + def adjoint(self, H: npt.ArrayLike) -> npt.ArrayLike: """ Args: - R (npt.ArrayLike): Rotation matrix + H (npt.ArrayLike): Homogeneous transform + Returns: + npt.ArrayLike: adjoint matrix + """ + R = H[:3, :3] + p = H[:3, 3] + X = self.factory.eye(6) + X[:3, :3] = R + X[3:6, 3:6] = R + X[:3, 3:6] = self.skew(p) @ R + return X + + def adjoint_inverse(self, H: npt.ArrayLike) -> npt.ArrayLike: + """ + Args: + H (npt.ArrayLike): Homogeneous transform + Returns: + npt.ArrayLike: adjoint matrix + """ + R = H[:3, :3] + p = H[:3, 3] + X = self.factory.eye(6) + X[:3, :3] = R.T + X[3:6, 3:6] = R.T + X[:3, 3:6] = -R.T @ self.skew(p) + return X + + def adjoint_mixed(self, H: npt.ArrayLike) -> npt.ArrayLike: + """ + Args: + H (npt.ArrayLike): Homogeneous transform + Returns: + npt.ArrayLike: adjoint matrix + """ + R = H[:3, :3] + X = self.factory.eye(6) + X[:3, :3] = R + X[3:6, 3:6] = R + return X + + def adjoint_mixed_inverse(self, H: npt.ArrayLike) -> npt.ArrayLike: + """ + Args: + H (npt.ArrayLike): Homogeneous transform Returns: npt.ArrayLike: adjoint matrix """ + R = H[:3, :3] X = self.factory.eye(6) X[:3, :3] = R.T X[3:6, 3:6] = R.T From 4dafc2c408f7ff946c19c3d1245d5cb4ed0560a1 Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 4 Aug 2023 11:37:56 +0200 Subject: [PATCH 09/39] Adapt interfaces --- src/adam/casadi/computations.py | 11 +++++++++++ src/adam/jax/computations.py | 11 +++++++++++ src/adam/numpy/computations.py | 11 +++++++++++ src/adam/pytorch/computations.py | 11 +++++++++++ 4 files changed, 44 insertions(+) diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index 6d8ed568..79e7ddc7 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -7,6 +7,7 @@ from adam.casadi.casadi_like import SpatialMath from adam.core import RBDAlgorithms +from adam.core.constants import Representations from adam.model import Model, URDFModelFactory @@ -37,6 +38,16 @@ def __init__( self.g = gravity self.f_opts = f_opts + def set_frame_velocity_representation( + self, representation: Representations + ) -> None: + """Sets the representation of the velocity of the frames + + Args: + representation (Representations): The representation of the velocity + """ + self.rbdalgos.set_frame_velocity_representation(representation) + def mass_matrix_fun(self) -> cs.Function: """Returns the Mass Matrix functions computed the CRBA diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index eaf64407..0281e43c 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -6,6 +6,7 @@ import numpy as np from jax import grad, jit, vmap +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, URDFModelFactory @@ -36,6 +37,16 @@ def __init__( self.NDoF = self.rbdalgos.NDoF self.g = gravity + def set_frame_velocity_representation( + self, representation: Representations + ) -> None: + """Sets the representation of the velocity of the frames + + Args: + representation (Representations): The representation of the velocity + """ + self.rbdalgos.set_frame_velocity_representation(representation) + def mass_matrix(self, base_transform: jnp.array, joint_positions: jnp.array): """Returns the Mass Matrix functions computed the CRBA diff --git a/src/adam/numpy/computations.py b/src/adam/numpy/computations.py index e0fe77d6..b2e9f430 100644 --- a/src/adam/numpy/computations.py +++ b/src/adam/numpy/computations.py @@ -4,6 +4,7 @@ import numpy as np +from adam.core.constants import Representations from adam.core.rbd_algorithms import RBDAlgorithms from adam.model import Model, URDFModelFactory from adam.numpy.numpy_like import SpatialMath @@ -34,6 +35,16 @@ def __init__( self.NDoF = model.NDoF self.g = gravity + def set_frame_velocity_representation( + self, representation: Representations + ) -> None: + """Sets the representation of the velocity of the frames + + Args: + representation (Representations): The representation of the velocity + """ + self.rbdalgos.set_frame_velocity_representation(representation) + def mass_matrix( self, base_transform: np.ndarray, joint_positions: np.ndarray ) -> np.ndarray: diff --git a/src/adam/pytorch/computations.py b/src/adam/pytorch/computations.py index d2f257a9..0e6762b4 100644 --- a/src/adam/pytorch/computations.py +++ b/src/adam/pytorch/computations.py @@ -5,6 +5,7 @@ import numpy as np import torch +from adam.core.constants import Representations from adam.core.rbd_algorithms import RBDAlgorithms from adam.model import Model, URDFModelFactory from adam.pytorch.torch_like import SpatialMath @@ -35,6 +36,16 @@ def __init__( self.NDoF = self.rbdalgos.NDoF self.g = gravity + def set_frame_velocity_representation( + self, representation: Representations + ) -> None: + """Sets the representation of the velocity of the frames + + Args: + representation (Representations): The representation of the velocity + """ + self.rbdalgos.set_frame_velocity_representation(representation) + def mass_matrix( self, base_transform: torch.Tensor, s: torch.Tensor ) -> torch.Tensor: From 97a7a82f703ce67f29ba0fe34bc4a421745a69dc Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 4 Aug 2023 11:38:26 +0200 Subject: [PATCH 10/39] Move and adapt tests --- ...tations.py => CasADi_computations_body.py} | 5 +- tests/NumPy_computations_body.py | 206 ++++++++++++++++ tests/body_fixed/test_CasADi_body_fixed.py | 219 ++++++++++++++++++ tests/body_fixed/test_Jax_body_fixed.py | 204 ++++++++++++++++ tests/body_fixed/test_NumPy_body_fixed.py | 210 +++++++++++++++++ tests/body_fixed/test_pytorch_body_fixed.py | 212 +++++++++++++++++ tests/mixed/test_CasADi_mixed.py | 215 +++++++++++++++++ .../test_Jax_mixed.py} | 0 .../test_NumPy_mixed.py} | 0 .../test_pytorch_mixed.py} | 0 10 files changed, 1270 insertions(+), 1 deletion(-) rename tests/{test_CasADi_computations.py => CasADi_computations_body.py} (96%) create mode 100644 tests/NumPy_computations_body.py create mode 100644 tests/body_fixed/test_CasADi_body_fixed.py create mode 100644 tests/body_fixed/test_Jax_body_fixed.py create mode 100644 tests/body_fixed/test_NumPy_body_fixed.py create mode 100644 tests/body_fixed/test_pytorch_body_fixed.py create mode 100644 tests/mixed/test_CasADi_mixed.py rename tests/{test_Jax_computations.py => mixed/test_Jax_mixed.py} (100%) rename tests/{test_NumPy_computations.py => mixed/test_NumPy_mixed.py} (100%) rename tests/{test_pytorch_computations.py => mixed/test_pytorch_mixed.py} (100%) diff --git a/tests/test_CasADi_computations.py b/tests/CasADi_computations_body.py similarity index 96% rename from tests/test_CasADi_computations.py rename to tests/CasADi_computations_body.py index 26262e42..92473bae 100644 --- a/tests/test_CasADi_computations.py +++ b/tests/CasADi_computations_body.py @@ -10,6 +10,7 @@ import numpy as np import pytest +import adam from adam.casadi import KinDynComputations from adam.geometry import utils @@ -59,13 +60,15 @@ def H_from_Pos_RPY_idyn(xyz, rpy): root_link = "root_link" comp = KinDynComputations(model_path, joints_name_list, root_link) +comp.set_frame_velocity_representation(adam.Representations.BODY_FIXED_REPRESENTATION) robot_iDyn = idyntree.ModelLoader() robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) kinDyn = idyntree.KinDynComputations() kinDyn.loadRobotModel(robot_iDyn.model()) kinDyn.setFloatingBase(root_link) -kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) +# kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) +kinDyn.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) n_dofs = len(joints_name_list) # base pose quantities diff --git a/tests/NumPy_computations_body.py b/tests/NumPy_computations_body.py new file mode 100644 index 00000000..d0e3fe4f --- /dev/null +++ b/tests/NumPy_computations_body.py @@ -0,0 +1,206 @@ +# 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 icub_models +import idyntree.swig as idyntree +import numpy as np +import pytest + +from adam import Representations +from adam.geometry import utils +from adam.numpy import KinDynComputations + +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.rbdalgos.set_frame_velocity_representation( + Representations.BODY_FIXED_REPRESENTATION +) + +robot_iDyn = idyntree.ModelLoader() +robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) +kinDyn = idyntree.KinDynComputations() +kinDyn.loadRobotModel(robot_iDyn.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 + +# set iDynTree kinDyn +H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) +vb = idyntree.Twist.Zero() +[vb.setVal(i, base_vel[i]) for i in range(6)] + +s = idyntree.VectorDynSize(n_dofs) +[s.setVal(i, joints_val[i]) for i in range(n_dofs)] +s_dot = idyntree.VectorDynSize(n_dofs) +[s_dot.setVal(i, joints_dot_val[i]) for i in range(n_dofs)] + +g = idyntree.Vector3() +g.zero() +g.setVal(2, -9.80665) +kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) +# set ADAM +H_b = utils.H_from_Pos_RPY(xyz, rpy) +vb_ = base_vel +s_ = joints_val +s_dot_ = joints_dot_val + + +def test_mass_matrix(): + mass_mx = idyntree.MatrixDynSize() + kinDyn.getFreeFloatingMassMatrix(mass_mx) + mass_mxNumpy = mass_mx.toNumPy() + mass_test = comp.mass_matrix(H_b, s_) + assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(): + cmm_idyntree = idyntree.MatrixDynSize() + kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) + cmm_idyntreeNumpy = cmm_idyntree.toNumPy() + Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) + assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(): + CoM_test = comp.CoM_position(H_b, s_) + CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() + assert CoM_test - 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(): + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_test = comp.jacobian("l_sole", H_b, s_) + assert iDynNumpyJ_ - J_test == 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_test = comp.jacobian("head", H_b, s_) + assert iDynNumpyJ_ - J_test == 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() + H_test = comp.forward_kinematics("l_sole", H_b, s_) + 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) + + +def test_fk_non_actuated(): + H_idyntree = kinDyn.getWorldTransform("head") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + H_test = comp.forward_kinematics("head", H_b, s_) + 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) + + +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_test = comp.bias_force(H_b, s_, vb_, s_dot_) + assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + g0 = idyntree.Vector3() + g0.zero() + kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, 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_test = comp.coriolis_term(H_b, s_, vb_, s_dot_) + assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) + + +# def test_gravity_term(): +kinDyn2 = idyntree.KinDynComputations() +kinDyn2.loadRobotModel(robot_iDyn.model()) +kinDyn2.setFloatingBase(root_link) +kinDyn2.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) +vb0 = idyntree.Twist.Zero() +s_dot0 = idyntree.VectorDynSize(n_dofs) +kinDyn2.setRobotState(H_b_idyn, s, vb0, s_dot0, g) +G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) +assert kinDyn2.generalizedBiasForces(G_iDyn) +G_iDyn_np = np.concatenate( + (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) +) +G_test = comp.gravity_term(H_b, s_) + +print("idyn", G_iDyn_np) +print("test", G_test) + +assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/body_fixed/test_CasADi_body_fixed.py b/tests/body_fixed/test_CasADi_body_fixed.py new file mode 100644 index 00000000..39dda4b2 --- /dev/null +++ b/tests/body_fixed/test_CasADi_body_fixed.py @@ -0,0 +1,219 @@ +# 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 + +import adam +from adam.casadi import KinDynComputations +from adam.geometry import utils + +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(adam.Representations.BODY_FIXED_REPRESENTATION) + +robot_iDyn = idyntree.ModelLoader() +robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) + +kinDyn = idyntree.KinDynComputations() +kinDyn.loadRobotModel(robot_iDyn.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 + +# set iDynTree kinDyn +H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) +vb = idyntree.Twist.Zero() +[vb.setVal(i, base_vel[i]) for i in range(6)] + +s = idyntree.VectorDynSize(n_dofs) +[s.setVal(i, joints_val[i]) for i in range(n_dofs)] +s_dot = idyntree.VectorDynSize(n_dofs) +[s_dot.setVal(i, joints_dot_val[i]) for i in range(n_dofs)] + +g = idyntree.Vector3() +g.zero() +g.setVal(2, -9.80665) +kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) +# set ADAM +H_b = utils.H_from_Pos_RPY(xyz, rpy) +vb_ = base_vel +s_ = joints_val +s_dot_ = joints_dot_val + + +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, s_)) + assert mass_test - 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, s_)) + assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(): + com_f = comp.CoM_position_fun() + CoM_cs = cs.DM(com_f(H_b, s_)) + CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() + assert CoM_cs - 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, s_)) + assert iDynNumpyJ_ - J_test == 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, s_)) + assert iDynNumpyJ_ - J_test == 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, s_)) + 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) + + +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, s_)) + 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) + + +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, s_, vb_, s_dot_)) + assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + g0 = idyntree.Vector3() + g0.zero() + kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, 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, s_, vb_, s_dot_)) + assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(): + vb0 = idyntree.Twist.Zero() + s_dot0 = idyntree.VectorDynSize(n_dofs) + kinDyn.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) + kinDyn.setRobotState(H_b_idyn, s, vb0, s_dot0, 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, s_)) + assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) + + +def test_relative_jacobian(): + kinDyn.setRobotState(H_from_Pos_RPY_idyn(np.zeros(3), np.zeros(3)), s, vb, s_dot, 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(s_)) + assert iDynNumpyRelativeJ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) diff --git a/tests/body_fixed/test_Jax_body_fixed.py b/tests/body_fixed/test_Jax_body_fixed.py new file mode 100644 index 00000000..f60394fe --- /dev/null +++ b/tests/body_fixed/test_Jax_body_fixed.py @@ -0,0 +1,204 @@ +# 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 icub_models +import idyntree.swig as idyntree +import jax.numpy as jnp +import numpy as np +import pytest +from jax import config + +import adam +from adam.geometry import utils +from adam.jax import KinDynComputations + +np.random.seed(42) +config.update("jax_enable_x64", True) + +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(adam.Representations.BODY_FIXED_REPRESENTATION) + +robot_iDyn = idyntree.ModelLoader() +robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) + +kinDyn = idyntree.KinDynComputations() +kinDyn.loadRobotModel(robot_iDyn.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 + +# set iDynTree kinDyn +H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) +vb = idyntree.Twist.Zero() +[vb.setVal(i, base_vel[i]) for i in range(6)] + +s = idyntree.VectorDynSize(n_dofs) +s = s.FromPython(joints_val) +s_dot = idyntree.VectorDynSize(n_dofs) +s_dot = s_dot.FromPython(joints_dot_val) + +g = idyntree.Vector3() +g.zero() +g.setVal(2, -9.80665) +kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) +# set ADAM +H_b = utils.H_from_Pos_RPY(xyz, rpy) +vb_ = base_vel +s_ = joints_val +s_dot_ = joints_dot_val + + +# def test_mass_matrix(): +mass_mx = idyntree.MatrixDynSize() +kinDyn.getFreeFloatingMassMatrix(mass_mx) +mass_mxNumpy = mass_mx.toNumPy() +mass_test = comp.mass_matrix(H_b, s_) +assert np.asarray(mass_test) - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(): + cmm_idyntree = idyntree.MatrixDynSize() + kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) + cmm_idyntreeNumpy = cmm_idyntree.toNumPy() + Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) + assert np.array(Jcm_test) - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(): + CoM_test = comp.CoM_position(H_b, s_) + CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() + assert CoM_test - 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(): + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_test = comp.jacobian("l_sole", H_b, s_) + assert iDynNumpyJ_ - np.array(J_test) == 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_test = comp.jacobian("head", H_b, s_) + assert iDynNumpyJ_ - np.array(J_test) == 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() + H_test = comp.forward_kinematics("l_sole", H_b, s_) + assert R_idy2np - np.array(H_test[:3, :3]) == pytest.approx(0.0, abs=1e-5) + assert p_idy2np - np.array(H_test[: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() + H_test = comp.forward_kinematics("head", H_b, s_) + 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) + + +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_test = comp.bias_force(H_b, s_, vb_, s_dot_) + assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + g0 = idyntree.Vector3() + g0.zero() + kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, 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_test = comp.coriolis_term(H_b, s_, vb_, s_dot_) + assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(): + kinDyn2 = idyntree.KinDynComputations() + kinDyn2.loadRobotModel(robot_iDyn.model()) + kinDyn2.setFloatingBase(root_link) + kinDyn2.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) + vb0 = idyntree.Twist.Zero() + s_dot0 = idyntree.VectorDynSize(n_dofs) + kinDyn2.setRobotState(H_b_idyn, s, vb0, s_dot0, g) + G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) + assert kinDyn2.generalizedBiasForces(G_iDyn) + G_iDyn_np = np.concatenate( + (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) + ) + G_test = comp.gravity_term(H_b, s_) + assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/body_fixed/test_NumPy_body_fixed.py b/tests/body_fixed/test_NumPy_body_fixed.py new file mode 100644 index 00000000..1a8d4534 --- /dev/null +++ b/tests/body_fixed/test_NumPy_body_fixed.py @@ -0,0 +1,210 @@ +# 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 icub_models +import idyntree.swig as idyntree +import numpy as np +import pytest + +import adam +from adam.geometry import utils +from adam.numpy import KinDynComputations + +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(adam.Representations.BODY_FIXED_REPRESENTATION) + +robot_iDyn = idyntree.ModelLoader() +robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) + +kinDyn = idyntree.KinDynComputations() +kinDyn.loadRobotModel(robot_iDyn.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 + +# set iDynTree kinDyn +H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) +vb = idyntree.Twist.Zero() +[vb.setVal(i, base_vel[i]) for i in range(6)] + +s = idyntree.VectorDynSize(n_dofs) +[s.setVal(i, joints_val[i]) for i in range(n_dofs)] +s_dot = idyntree.VectorDynSize(n_dofs) +[s_dot.setVal(i, joints_dot_val[i]) for i in range(n_dofs)] + +g = idyntree.Vector3() +g.zero() +g.setVal(2, -9.80665) +kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) +# set ADAM +H_b = utils.H_from_Pos_RPY(xyz, rpy) +vb_ = base_vel +s_ = joints_val +s_dot_ = joints_dot_val + + +def test_mass_matrix(): + mass_mx = idyntree.MatrixDynSize() + kinDyn.getFreeFloatingMassMatrix(mass_mx) + mass_mxNumpy = mass_mx.toNumPy() + mass_test = comp.mass_matrix(H_b, s_) + assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(): + cmm_idyntree = idyntree.MatrixDynSize() + kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) + cmm_idyntreeNumpy = cmm_idyntree.toNumPy() + Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) + assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(): + CoM_test = comp.CoM_position(H_b, s_) + CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() + assert CoM_test - 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(): + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_test = comp.jacobian("l_sole", H_b, s_) + assert iDynNumpyJ_ - J_test == 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_test = comp.jacobian("head", H_b, s_) + assert iDynNumpyJ_ - J_test == 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() + H_test = comp.forward_kinematics("l_sole", H_b, s_) + 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) + + +def test_fk_non_actuated(): + H_idyntree = kinDyn.getWorldTransform("head") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + H_test = comp.forward_kinematics("head", H_b, s_) + 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) + + +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_test = comp.bias_force(H_b, s_, vb_, s_dot_) + assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + g0 = idyntree.Vector3() + g0.zero() + kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, 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_test = comp.coriolis_term(H_b, s_, vb_, s_dot_) + assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(): + kinDyn2 = idyntree.KinDynComputations() + kinDyn2.loadRobotModel(robot_iDyn.model()) + kinDyn2.setFloatingBase(root_link) + kinDyn2.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) + vb0 = idyntree.Twist.Zero() + s_dot0 = idyntree.VectorDynSize(n_dofs) + kinDyn2.setRobotState(H_b_idyn, s, vb0, s_dot0, g) + G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) + assert kinDyn2.generalizedBiasForces(G_iDyn) + G_iDyn_np = np.concatenate( + (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) + ) + G_test = comp.gravity_term(H_b, s_) + assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) + + +def test_relative_jacobian(): + kinDyn.setRobotState(H_from_Pos_RPY_idyn(np.zeros(3), np.zeros(3)), s, vb, s_dot, g) + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) + iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] + J_test = comp.relative_jacobian("l_sole", s_) + assert iDynNumpyRelativeJ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) diff --git a/tests/body_fixed/test_pytorch_body_fixed.py b/tests/body_fixed/test_pytorch_body_fixed.py new file mode 100644 index 00000000..f19a1fee --- /dev/null +++ b/tests/body_fixed/test_pytorch_body_fixed.py @@ -0,0 +1,212 @@ +# 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 icub_models +import idyntree.swig as idyntree +import numpy as np +import pytest +import torch + +import adam +from adam.geometry import utils +from adam.pytorch import KinDynComputations + +np.random.seed(42) +torch.set_default_dtype(torch.float64) + +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(adam.Representations.BODY_FIXED_REPRESENTATION) + +robot_iDyn = idyntree.ModelLoader() +robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) + +kinDyn = idyntree.KinDynComputations() +kinDyn.loadRobotModel(robot_iDyn.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 + +# set iDynTree kinDyn +H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) +vb = idyntree.Twist.Zero() +[vb.setVal(i, base_vel[i]) for i in range(6)] + +s = idyntree.VectorDynSize(n_dofs) +[s.setVal(i, joints_val[i]) for i in range(n_dofs)] +s_dot = idyntree.VectorDynSize(n_dofs) +[s_dot.setVal(i, joints_dot_val[i]) for i in range(n_dofs)] + +g = idyntree.Vector3() +g.zero() +g.setVal(2, -9.80665) +kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) +# set ADAM +H_b = torch.FloatTensor(utils.H_from_Pos_RPY(xyz, rpy)) +vb_ = torch.FloatTensor(base_vel) +s_ = torch.FloatTensor(joints_val) +s_dot_ = torch.FloatTensor(joints_dot_val) + + +def test_mass_matrix(): + mass_mx = idyntree.MatrixDynSize() + kinDyn.getFreeFloatingMassMatrix(mass_mx) + mass_mxNumpy = mass_mx.toNumPy() + mass_test = comp.mass_matrix(H_b, s_) + assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-4) + + +def test_CMM(): + cmm_idyntree = idyntree.MatrixDynSize() + kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) + cmm_idyntreeNumpy = cmm_idyntree.toNumPy() + Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) + assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-4) + + +def test_CoM_pos(): + CoM_test = comp.CoM_position(H_b, s_) + CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() + assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-4) + + +def test_total_mass(): + assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx( + 0.0, abs=1e-4 + ) + + +def test_jacobian(): + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_test = comp.jacobian("l_sole", H_b, s_) + assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) + + +def test_jacobian_non_actuated(): + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_test = comp.jacobian("head", H_b, s_) + assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) + + +def test_fk(): + H_idyntree = kinDyn.getWorldTransform("l_sole") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + H_test = comp.forward_kinematics("l_sole", H_b, s_) + assert (R_idy2np) - np.array(H_test[:3, :3]) == pytest.approx(0.0, abs=1e-4) + assert (p_idy2np) - np.array(H_test[:3, 3]) == pytest.approx(0.0, abs=1e-4) + + +def test_fk_non_actuated(): + H_idyntree = kinDyn.getWorldTransform("head") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + H_test = comp.forward_kinematics("head", H_b, s_) + assert R_idy2np - np.array(H_test[:3, :3]) == pytest.approx(0.0, abs=1e-4) + assert p_idy2np - np.array(H_test[:3, 3]) == pytest.approx(0.0, abs=1e-4) + + +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_test = comp.bias_force(H_b, s_, vb_, s_dot_) + assert h_iDyn_np - np.array(h_test) == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + g0 = idyntree.Vector3() + g0.zero() + kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, 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_test = comp.coriolis_term(H_b, s_, vb_, s_dot_) + assert C_iDyn_np - np.array(C_test) == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(): + kinDyn2 = idyntree.KinDynComputations() + kinDyn2.loadRobotModel(robot_iDyn.model()) + kinDyn2.setFloatingBase(root_link) + kinDyn2.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) + vb0 = idyntree.Twist.Zero() + s_dot0 = idyntree.VectorDynSize(n_dofs) + kinDyn2.setRobotState(H_b_idyn, s, vb0, s_dot0, g) + G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) + assert kinDyn2.generalizedBiasForces(G_iDyn) + G_iDyn_np = np.concatenate( + (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) + ) + G_test = comp.gravity_term(H_b, s_) + assert G_iDyn_np - np.array(G_test) == pytest.approx(0.0, abs=1e-4) + + +def test_relative_jacobian(): + kinDyn.setRobotState(H_from_Pos_RPY_idyn(np.zeros(3), np.zeros(3)), s, vb, s_dot, g) + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) + iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] + J_test = comp.relative_jacobian("l_sole", s_) + assert iDynNumpyRelativeJ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_CasADi_mixed.py b/tests/mixed/test_CasADi_mixed.py new file mode 100644 index 00000000..445f16b4 --- /dev/null +++ b/tests/mixed/test_CasADi_mixed.py @@ -0,0 +1,215 @@ +# 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 + +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) +robot_iDyn = idyntree.ModelLoader() +robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) + +kinDyn = idyntree.KinDynComputations() +kinDyn.loadRobotModel(robot_iDyn.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 quantitites +joints_val = (np.random.rand(n_dofs) - 0.5) * 5 +joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 + +# set iDynTree kinDyn +H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) +vb = idyntree.Twist.Zero() +[vb.setVal(i, base_vel[i]) for i in range(6)] + +s = idyntree.VectorDynSize(n_dofs) +[s.setVal(i, joints_val[i]) for i in range(n_dofs)] +s_dot = idyntree.VectorDynSize(n_dofs) +[s_dot.setVal(i, joints_dot_val[i]) for i in range(n_dofs)] + +g = idyntree.Vector3() +g.zero() +g.setVal(2, -9.80665) +kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) +# set ADAM +H_b = utils.H_from_Pos_RPY(xyz, rpy) +vb_ = base_vel +s_ = joints_val +s_dot_ = joints_dot_val + + +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, s_)) + assert mass_test - 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, s_)) + assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(): + com_f = comp.CoM_position_fun() + CoM_cs = cs.DM(com_f(H_b, s_)) + CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() + assert CoM_cs - 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, s_)) + assert iDynNumpyJ_ - J_test == 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, s_)) + assert iDynNumpyJ_ - J_test == 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, s_)) + 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) + + +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, s_)) + 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) + + +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, s_, vb_, s_dot_)) + assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + g0 = idyntree.Vector3() + g0.zero() + kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, 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, s_, vb_, s_dot_)) + assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(): + vb0 = idyntree.Twist.Zero() + s_dot0 = idyntree.VectorDynSize(n_dofs) + kinDyn.setRobotState(H_b_idyn, s, vb0, s_dot0, 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, s_)) + assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) + + +def test_relative_jacobian(): + kinDyn.setRobotState(H_from_Pos_RPY_idyn(np.zeros(3), np.zeros(3)), s, vb, s_dot, g) + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) + iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] + J = comp.relative_jacobian_fun("l_sole") + J_test = cs.DM(J(s_)) + assert iDynNumpyRelativeJ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) diff --git a/tests/test_Jax_computations.py b/tests/mixed/test_Jax_mixed.py similarity index 100% rename from tests/test_Jax_computations.py rename to tests/mixed/test_Jax_mixed.py diff --git a/tests/test_NumPy_computations.py b/tests/mixed/test_NumPy_mixed.py similarity index 100% rename from tests/test_NumPy_computations.py rename to tests/mixed/test_NumPy_mixed.py diff --git a/tests/test_pytorch_computations.py b/tests/mixed/test_pytorch_mixed.py similarity index 100% rename from tests/test_pytorch_computations.py rename to tests/mixed/test_pytorch_mixed.py From 3ee01b5abecc4299aab7d68ee7a2fd53e2407b05 Mon Sep 17 00:00:00 2001 From: giulero Date: Tue, 10 Oct 2023 19:06:05 +0200 Subject: [PATCH 11/39] Add adjoint derivatives --- src/adam/core/spatial_math.py | 87 +++++++++++++++++++++++++++++++++++ 1 file changed, 87 insertions(+) diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py index 6dbf6a10..19e23284 100644 --- a/src/adam/core/spatial_math.py +++ b/src/adam/core/spatial_math.py @@ -419,6 +419,25 @@ def adjoint(self, H: npt.ArrayLike) -> npt.ArrayLike: X[:3, 3:6] = self.skew(p) @ R return X + def adjoint_derivative(self, H: npt.ArrayLike, v: npt.ArrayLike) -> npt.ArrayLike: + """ + Args: + H (npt.ArrayLike): Homogeneous transform + v (npt.ArrayLike): 6D twist + Returns: + npt.ArrayLike: adjoint matrix derivative + """ + + R = H[:3, :3] + p = H[:3, 3] + R_dot = self.skew(v[3:]) @ R + p_dot = v[:3] - self.skew(p) @ v[3:] + X = self.factory.zeros(6, 6) + X[:3, :3] = R_dot + X[3:6, 3:6] = R_dot + X[:3, 3:6] = self.skew(p_dot) @ R + self.skew(p) @ R_dot + return X + def adjoint_inverse(self, H: npt.ArrayLike) -> npt.ArrayLike: """ Args: @@ -434,6 +453,26 @@ def adjoint_inverse(self, H: npt.ArrayLike) -> npt.ArrayLike: X[:3, 3:6] = -R.T @ self.skew(p) return X + def adjoint_inverse_derivative( + self, H: npt.ArrayLike, v: npt.ArrayLike + ) -> npt.ArrayLike: + """ + Args: + H (npt.ArrayLike): Homogeneous transform + v (npt.ArrayLike): 6D twist + Returns: + npt.ArrayLike: adjoint matrix derivative + """ + R = H[:3, :3] + p = H[:3, 3] + R_dot = self.skew(v[3:]) @ R + p_dot = v[:3] - self.skew(p) @ v[3:] + X = self.factory.zeros(6, 6) + X[:3, :3] = R_dot.T + X[3:6, 3:6] = R_dot.T + X[:3, 3:6] = -R_dot.T @ self.skew(p) - R.T @ self.skew(p_dot) + return X + def adjoint_mixed(self, H: npt.ArrayLike) -> npt.ArrayLike: """ Args: @@ -459,3 +498,51 @@ def adjoint_mixed_inverse(self, H: npt.ArrayLike) -> npt.ArrayLike: X[:3, :3] = R.T X[3:6, 3:6] = R.T return X + + def adjoint_mixed_derivative( + self, H: npt.ArrayLike, v: npt.ArrayLike + ) -> npt.ArrayLike: + """ + Args: + H (npt.ArrayLike): Homogeneous transform + v (npt.ArrayLike): 6D twist + Returns: + npt.ArrayLike: adjoint matrix derivative + """ + R = H[:3, :3] + R_dot = R @ self.skew(v[3:]) + X = self.factory.zeros(6, 6) + X[:3, :3] = R_dot + X[3:6, 3:6] = R_dot + return X + + def adjoint_mixed_inverse_derivative( + self, H: npt.ArrayLike, v: npt.ArrayLike + ) -> npt.ArrayLike: + """ + Args: + H (npt.ArrayLike): Homogeneous transform + v (npt.ArrayLike): 6D twist + Returns: + npt.ArrayLike: adjoint matrix derivative + """ + R = H[:3, :3] + R_dot = R @ self.skew(v[3:]) + X = self.factory.zeros(6, 6) + X[:3, :3] = R_dot.T + X[3:6, 3:6] = R_dot.T + return X + + def homogeneous_inverse(self, H: npt.ArrayLike) -> npt.ArrayLike: + """ + Args: + H (npt.ArrayLike): Homogeneous transform + Returns: + npt.ArrayLike: inverse of the homogeneous transform + """ + R = H[:3, :3] + p = H[:3, 3] + T = self.factory.eye(4) + T[:3, :3] = R.T + T[:3, 3] = -R.T @ p + return T From ea0873621372b2aa0a7a48ff94a637eb21fc1748 Mon Sep 17 00:00:00 2001 From: giulero Date: Tue, 10 Oct 2023 19:27:16 +0200 Subject: [PATCH 12/39] Change notation --- src/adam/core/rbd_algorithms.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 4039022c..81977324 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -140,16 +140,16 @@ def forward_kinematics( base_transform (npt.ArrayLike): The homogenous transform from base to world frame joint_positions (npt.ArrayLike): The joints position Returns: - T_fk (npt.ArrayLike): The fk represented as Homogenous transformation matrix + I_H_ee (npt.ArrayLike): The fk represented as Homogenous transformation matrix """ chain = self.model.get_joints_chain(self.root_link, frame) - T_fk = self.math.factory.eye(4) - T_fk = T_fk @ base_transform + I_H_ee = self.math.factory.eye(4) + I_H_ee = I_H_ee @ base_transform for joint in chain: q_ = joint_positions[joint.idx] if joint.idx is not None else 0.0 - T_joint = joint.homogeneous(q=q_) - T_fk = T_fk @ T_joint - return T_fk + H_joint = joint.homogeneous(q=q_) + I_H_ee = I_H_ee @ H_joint + return I_H_ee def joints_jacobian( self, frame: str, base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike From 5c7b5ec986d6fae12d483de68c1830d20b8919a2 Mon Sep 17 00:00:00 2001 From: giulero Date: Tue, 10 Oct 2023 19:27:44 +0200 Subject: [PATCH 13/39] Use different jacobian computation --- src/adam/core/rbd_algorithms.py | 42 ++++++++++++--------------------- 1 file changed, 15 insertions(+), 27 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 81977324..7b67589e 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -152,7 +152,7 @@ def forward_kinematics( return I_H_ee def joints_jacobian( - self, frame: str, base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike + self, frame: str, joint_positions: npt.ArrayLike ) -> npt.ArrayLike: """Returns the Jacobian relative to the specified frame @@ -165,40 +165,27 @@ def joints_jacobian( J (npt.ArrayLike): The Joints Jacobian relative to the frame """ chain = self.model.get_joints_chain(self.root_link, frame) - T_fk = self.math.factory.eye(4) @ base_transform + eye = self.math.factory.eye(4) + B_H_l = eye J = self.math.factory.zeros(6, self.NDoF) - T_ee = self.forward_kinematics(frame, base_transform, joint_positions) - P_ee = T_ee[:3, 3] + B_H_ee = self.forward_kinematics(frame, eye, joint_positions) + ee_H_B = self.math.homogeneous_inverse(B_H_ee) for joint in chain: - q_ = joint_positions[joint.idx] if joint.idx is not None else 0.0 - T_joint = joint.homogeneous(q=q_) - T_fk = T_fk @ T_joint - if joint.type in ["revolute", "continuous"]: - p_prev = P_ee - T_fk[:3, 3] - z_prev = T_fk[:3, :3] @ joint.axis - J_lin = self.math.skew(z_prev) @ p_prev - J_ang = z_prev - elif joint.type in ["prismatic"]: - z_prev = T_fk[:3, :3] @ joint.axis - J_lin = z_prev - J_ang = self.math.factory.zeros(3) - + q = joint_positions[joint.idx] if joint.idx is not None else 0.0 + H_j = joint.homogeneous(q=q) + B_H_l = B_H_l @ H_j + ee_H_l = ee_H_B @ B_H_l if joint.idx is not None: - J[:, joint.idx] = self.math.vertcat(J_lin, J_ang) - + J[:, joint.idx] = self.math.adjoint(ee_H_l) @ joint.motion_subspace() return J def jacobian( self, frame: str, base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike ) -> npt.ArrayLike: eye = self.math.factory.eye(4) - J = self.joints_jacobian(frame, eye, joint_positions) - T_ee = self.forward_kinematics(frame, eye, joint_positions) - X = self.math.factory.zeros(6, 6) - L_X_B = self.math.adjoint_inverse(T_ee) - J = ( - self.math.adjoint_mixed_inverse(T_ee) @ J - ) # J for now is B_J_L, we need L_J_L, so we need to multiply by L_R_B + J = self.joints_jacobian(frame, joint_positions) + B_H_ee = self.forward_kinematics(frame, eye, joint_positions) + L_X_B = self.math.adjoint_inverse(B_H_ee) J_tot = self.math.factory.zeros(6, self.NDoF + 6) J_tot[:6, :6] = L_X_B J_tot[:, 6:] = J @@ -210,7 +197,7 @@ def jacobian( return J_tot # let's move to mixed representation elif self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: - w_H_L = base_transform @ T_ee.array + w_H_L = base_transform @ B_H_ee.array LI_X_L = self.math.adjoint_mixed(w_H_L) X = self.math.factory.eye(6 + self.NDoF) X[:6, :6] = self.math.adjoint_mixed_inverse(base_transform) @@ -233,6 +220,7 @@ def relative_jacobian( Returns: J (npt.ArrayLike): The Jacobian between the root and the frame """ + return self.joints_jacobian(frame, joint_positions) eye = self.math.factory.eye(4) T_ee = self.forward_kinematics(frame, eye, joint_positions) if self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: From 3b216300e1dcb3cde44e3df9da9627cffab73811 Mon Sep 17 00:00:00 2001 From: giulero Date: Tue, 10 Oct 2023 19:28:03 +0200 Subject: [PATCH 14/39] Compute J_dot --- src/adam/core/rbd_algorithms.py | 70 +++++++++++++++++++++++++++++---- 1 file changed, 63 insertions(+), 7 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 7b67589e..46ff0231 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -221,18 +221,74 @@ def relative_jacobian( J (npt.ArrayLike): The Jacobian between the root and the frame """ return self.joints_jacobian(frame, joint_positions) + + def jacobian_dot( + self, + frame: str, + base_transform: npt.ArrayLike, + joint_positions: npt.ArrayLike, + base_velocity: npt.ArrayLike, + joint_velocities: npt.ArrayLike, + ) -> npt.ArrayLike: + """Returns the Jacobian derivative relative to the specified frame + + Args: + 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 + joint_velocities (npt.ArrayLike): The joints velocity + + Returns: + J_dot (npt.ArrayLike): The Jacobian derivative relative to the frame + """ + chain = self.model.get_joints_chain(self.root_link, frame) eye = self.math.factory.eye(4) - T_ee = self.forward_kinematics(frame, eye, joint_positions) - if self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: - return self.joints_jacobian(frame, eye, joint_positions) + B_H_l = eye + J = self.math.factory.zeros(6, self.NDoF + 6) + J_dot = self.math.factory.zeros(6, self.NDoF + 6) + B_H_ee = self.forward_kinematics(frame, eye, joint_positions) + ee_H_B = self.math.homogeneous_inverse(B_H_ee) + v = self.math.adjoint(ee_H_B) @ base_velocity + a = self.math.adjoint_derivative(ee_H_B, v) @ base_velocity + J[:, :6] = self.math.adjoint(ee_H_B) + J_dot[:, :6] = self.math.adjoint_derivative(ee_H_B, v) + for joint in chain: + q = joint_positions[joint.idx] if joint.idx is not None else 0.0 + q_dot = joint_velocities[joint.idx] if joint.idx is not None else 0.0 + H_j = joint.homogeneous(q=q) + B_H_l = B_H_l @ H_j + ee_H_l = ee_H_B @ B_H_l + J_j = self.math.adjoint(ee_H_l) @ joint.motion_subspace() + J_dot_j = self.math.adjoint_derivative(ee_H_l, v) @ joint.motion_subspace() + v += J_j * q_dot + a += J_dot_j * q_dot + if joint.idx is not None: + J[:, joint.idx + 6] = J_j + J_dot[:, joint.idx + 6] = J_dot_j - elif ( + if ( self.frame_velocity_representation == Representations.BODY_FIXED_REPRESENTATION ): - return self.math.adjoint_mixed_inverse(T_ee) @ self.joints_jacobian( - frame, eye, joint_positions - ) # J for now is B_J_L, we need L_J_L, so we need to multiply by L_R_B + return J_dot + # let's move to mixed representation + elif self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: + w_H_l = base_transform @ B_H_l.array + LI_X_l = self.math.adjoint_mixed(w_H_l) + X = self.math.factory.eye(6 + self.NDoF) + X[:6, :6] = self.math.adjoint_mixed_inverse(base_transform) + LI_X_l_dot = self.math.adjoint_mixed_derivative(w_H_l, v.array) + + X_dot = self.math.factory.zeros(6 + self.NDoF, 6 + self.NDoF) + X_dot[:6, :6] = self.math.adjoint_mixed_inverse_derivative( + base_transform, base_velocity + ) + derivative_1 = LI_X_l_dot @ J @ X + derivative_2 = LI_X_l @ J_dot @ X + derivative_3 = LI_X_l @ J @ X_dot + J_dot = derivative_1 + derivative_2 + derivative_3 + return J_dot else: raise NotImplementedError( "Only BODY_FIXED_REPRESENTATION and MIXED_REPRESENTATION are implemented" From 2b39c84f0bedd3a041f3b9a3b2ca293e7a9927b7 Mon Sep 17 00:00:00 2001 From: giulero Date: Tue, 10 Oct 2023 19:28:18 +0200 Subject: [PATCH 15/39] Update notation --- src/adam/core/rbd_algorithms.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 46ff0231..72f3befa 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -309,11 +309,11 @@ def CoM_position( com_pos = self.math.factory.zeros(3, 1) for item in self.model.tree: link = item.link - T_fk = self.forward_kinematics(link.name, base_transform, joint_positions) - T_link = link.homogeneous() + I_H_l = self.forward_kinematics(link.name, base_transform, joint_positions) + H_link = link.homogeneous() # Adding the link transform - T_fk = T_fk @ T_link - com_pos += T_fk[:3, 3] * link.inertial.mass + I_H_l = I_H_l @ H_link + com_pos += I_H_l[:3, 3] * link.inertial.mass com_pos /= self.get_total_mass() return com_pos From 9f06cab5aa7fa4a1250eb8cc2d3fdbdcda063c90 Mon Sep 17 00:00:00 2001 From: giulero Date: Tue, 10 Oct 2023 19:28:51 +0200 Subject: [PATCH 16/39] Add jacobian_dot function --- src/adam/numpy/computations.py | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/adam/numpy/computations.py b/src/adam/numpy/computations.py index b2e9f430..bf0e8759 100644 --- a/src/adam/numpy/computations.py +++ b/src/adam/numpy/computations.py @@ -121,6 +121,30 @@ def relative_jacobian(self, frame: str, joint_positions: np.ndarray) -> np.ndarr """ return self.rbdalgos.relative_jacobian(frame, joint_positions).array + def jacobian_dot( + self, + frame: str, + base_transform: np.ndarray, + joint_positions: np.ndarray, + base_velocity: np.ndarray, + joint_velocities: np.ndarray, + ) -> np.ndarray: + """Returns the Jacobian derivative relative to the specified frame + + Args: + frame (str): The frame to which the jacobian will be computed + base_transform (np.ndarray): The homogenous transform from base to world frame + joint_positions (np.ndarray): The joints position + base_velocity (np.ndarray): The base velocity in mixed representation + joint_velocities (np.ndarray): The joint velocities + + Returns: + Jdot (np.ndarray): The Jacobian derivative relative to the frame + """ + return self.rbdalgos.jacobian_dot( + frame, base_transform, joint_positions, base_velocity, joint_velocities + ).array.squeeze() + def CoM_position( self, base_transform: np.ndarray, joint_positions: np.ndarray ) -> np.ndarray: From b44775c854cfa78323e8724a00c15c781cb187da Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 11 Oct 2023 15:51:17 +0200 Subject: [PATCH 17/39] Fix transforms --- src/adam/core/spatial_math.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py index 19e23284..2c6bb7a4 100644 --- a/src/adam/core/spatial_math.py +++ b/src/adam/core/spatial_math.py @@ -510,7 +510,7 @@ def adjoint_mixed_derivative( npt.ArrayLike: adjoint matrix derivative """ R = H[:3, :3] - R_dot = R @ self.skew(v[3:]) + R_dot = self.skew(v[3:]) @ R X = self.factory.zeros(6, 6) X[:3, :3] = R_dot X[3:6, 3:6] = R_dot @@ -527,7 +527,7 @@ def adjoint_mixed_inverse_derivative( npt.ArrayLike: adjoint matrix derivative """ R = H[:3, :3] - R_dot = R @ self.skew(v[3:]) + R_dot = self.skew(v[3:]) @ R X = self.factory.zeros(6, 6) X[:3, :3] = R_dot.T X[3:6, 3:6] = R_dot.T From 8be3c88d976990c9e2576c30b6586439d0253476 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 11 Oct 2023 15:52:05 +0200 Subject: [PATCH 18/39] Fix jacobian dot to work also in mixed representation --- src/adam/core/rbd_algorithms.py | 58 +++++++++++++++++++++------------ 1 file changed, 37 insertions(+), 21 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 72f3befa..3e1aca22 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -244,24 +244,40 @@ def jacobian_dot( """ chain = self.model.get_joints_chain(self.root_link, frame) eye = self.math.factory.eye(4) - B_H_l = eye + # initialize the transform from the base to a generic link j in the chain + B_H_j = eye J = self.math.factory.zeros(6, self.NDoF + 6) J_dot = self.math.factory.zeros(6, self.NDoF + 6) - B_H_ee = self.forward_kinematics(frame, eye, joint_positions) - ee_H_B = self.math.homogeneous_inverse(B_H_ee) - v = self.math.adjoint(ee_H_B) @ base_velocity - a = self.math.adjoint_derivative(ee_H_B, v) @ base_velocity - J[:, :6] = self.math.adjoint(ee_H_B) - J_dot[:, :6] = self.math.adjoint_derivative(ee_H_B, v) + # The homogeneous transform from the base to the frame + B_H_L = self.forward_kinematics(frame, eye, joint_positions) + L_H_B = self.math.homogeneous_inverse(B_H_L) + + if self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: + # Convert base velocity from mixed to left trivialized representation + B_v_IB = self.math.adjoint_mixed_inverse(base_transform) @ base_velocity + elif ( + self.frame_velocity_representation + == Representations.BODY_FIXED_REPRESENTATION + ): + B_v_IB = base_velocity + else: + raise NotImplementedError( + "Only BODY_FIXED_REPRESENTATION and MIXED_REPRESENTATION are implemented" + ) + + v = self.math.adjoint(L_H_B) @ B_v_IB + a = self.math.adjoint_derivative(L_H_B, v) @ B_v_IB + J[:, :6] = self.math.adjoint(L_H_B) + J_dot[:, :6] = self.math.adjoint_derivative(L_H_B, v) for joint in chain: q = joint_positions[joint.idx] if joint.idx is not None else 0.0 q_dot = joint_velocities[joint.idx] if joint.idx is not None else 0.0 H_j = joint.homogeneous(q=q) - B_H_l = B_H_l @ H_j - ee_H_l = ee_H_B @ B_H_l - J_j = self.math.adjoint(ee_H_l) @ joint.motion_subspace() - J_dot_j = self.math.adjoint_derivative(ee_H_l, v) @ joint.motion_subspace() + B_H_j = B_H_j @ H_j + L_H_j = L_H_B @ B_H_j + J_j = self.math.adjoint(L_H_j) @ joint.motion_subspace() v += J_j * q_dot + J_dot_j = self.math.adjoint_derivative(L_H_j, v) @ joint.motion_subspace() a += J_dot_j * q_dot if joint.idx is not None: J[:, joint.idx + 6] = J_j @@ -274,19 +290,19 @@ def jacobian_dot( return J_dot # let's move to mixed representation elif self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: - w_H_l = base_transform @ B_H_l.array - LI_X_l = self.math.adjoint_mixed(w_H_l) + I_H_L = base_transform @ B_H_j.array + LI_X_L = self.math.adjoint_mixed(I_H_L) X = self.math.factory.eye(6 + self.NDoF) X[:6, :6] = self.math.adjoint_mixed_inverse(base_transform) - LI_X_l_dot = self.math.adjoint_mixed_derivative(w_H_l, v.array) - + I_H_L = self.forward_kinematics(frame, base_transform, joint_positions) + LI_v_L = self.math.adjoint_mixed(I_H_L) @ v # v = L_v_IL + LI_X_L_dot = self.math.adjoint_mixed_derivative(I_H_L, LI_v_L) X_dot = self.math.factory.zeros(6 + self.NDoF, 6 + self.NDoF) - X_dot[:6, :6] = self.math.adjoint_mixed_inverse_derivative( - base_transform, base_velocity - ) - derivative_1 = LI_X_l_dot @ J @ X - derivative_2 = LI_X_l @ J_dot @ X - derivative_3 = LI_X_l @ J @ X_dot + B_H_I = self.math.homogeneous_inverse(base_transform) + X_dot[:6, :6] = self.math.adjoint_mixed_derivative(B_H_I, -B_v_IB) + derivative_1 = LI_X_L_dot @ J @ X + derivative_2 = LI_X_L @ J_dot @ X + derivative_3 = LI_X_L @ J @ X_dot J_dot = derivative_1 + derivative_2 + derivative_3 return J_dot else: From 0fb5b2f0235adad4d821a5b5f584b645bb5bce3b Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 11 Oct 2023 16:06:52 +0200 Subject: [PATCH 19/39] Rename variable -> more expressive --- src/adam/core/rbd_algorithms.py | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 3e1aca22..ea0b514b 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -140,16 +140,16 @@ def forward_kinematics( base_transform (npt.ArrayLike): The homogenous transform from base to world frame joint_positions (npt.ArrayLike): The joints position Returns: - I_H_ee (npt.ArrayLike): The fk represented as Homogenous transformation matrix + I_H_L (npt.ArrayLike): The fk represented as Homogenous transformation matrix """ chain = self.model.get_joints_chain(self.root_link, frame) - I_H_ee = self.math.factory.eye(4) - I_H_ee = I_H_ee @ base_transform + I_H_L = self.math.factory.eye(4) + I_H_L = I_H_L @ base_transform for joint in chain: q_ = joint_positions[joint.idx] if joint.idx is not None else 0.0 H_joint = joint.homogeneous(q=q_) - I_H_ee = I_H_ee @ H_joint - return I_H_ee + I_H_L = I_H_L @ H_joint + return I_H_L def joints_jacobian( self, frame: str, joint_positions: npt.ArrayLike @@ -166,17 +166,17 @@ def joints_jacobian( """ chain = self.model.get_joints_chain(self.root_link, frame) eye = self.math.factory.eye(4) - B_H_l = eye + B_H_j = eye J = self.math.factory.zeros(6, self.NDoF) - B_H_ee = self.forward_kinematics(frame, eye, joint_positions) - ee_H_B = self.math.homogeneous_inverse(B_H_ee) + B_H_L = self.forward_kinematics(frame, eye, joint_positions) + L_H_B = self.math.homogeneous_inverse(B_H_L) for joint in chain: q = joint_positions[joint.idx] if joint.idx is not None else 0.0 H_j = joint.homogeneous(q=q) - B_H_l = B_H_l @ H_j - ee_H_l = ee_H_B @ B_H_l + B_H_j = B_H_j @ H_j + L_H_j = L_H_B @ B_H_j if joint.idx is not None: - J[:, joint.idx] = self.math.adjoint(ee_H_l) @ joint.motion_subspace() + J[:, joint.idx] = self.math.adjoint(L_H_j) @ joint.motion_subspace() return J def jacobian( @@ -184,8 +184,8 @@ def jacobian( ) -> npt.ArrayLike: eye = self.math.factory.eye(4) J = self.joints_jacobian(frame, joint_positions) - B_H_ee = self.forward_kinematics(frame, eye, joint_positions) - L_X_B = self.math.adjoint_inverse(B_H_ee) + B_H_L = self.forward_kinematics(frame, eye, joint_positions) + L_X_B = self.math.adjoint_inverse(B_H_L) J_tot = self.math.factory.zeros(6, self.NDoF + 6) J_tot[:6, :6] = L_X_B J_tot[:, 6:] = J @@ -197,7 +197,7 @@ def jacobian( return J_tot # let's move to mixed representation elif self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: - w_H_L = base_transform @ B_H_ee.array + w_H_L = base_transform @ B_H_L.array LI_X_L = self.math.adjoint_mixed(w_H_L) X = self.math.factory.eye(6 + self.NDoF) X[:6, :6] = self.math.adjoint_mixed_inverse(base_transform) From e73216c2213b7a16bb169184f55b80ac56f8c3c2 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 11 Oct 2023 16:07:09 +0200 Subject: [PATCH 20/39] Fix relative jacobian --- src/adam/core/rbd_algorithms.py | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index ea0b514b..491c6f20 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -211,16 +211,28 @@ def jacobian( def relative_jacobian( self, frame: str, joint_positions: npt.ArrayLike ) -> npt.ArrayLike: - """Returns the Jacobian between the root link and a specified frame frames - + """Returns the Jacobian between the root link and a specified frame Args: frame (str): The tip of the chain joint_positions (npt.ArrayLike): The joints position Returns: - J (npt.ArrayLike): The Jacobian between the root and the frame + J (npt.ArrayLike): The 6 x NDoF Jacobian between the root and the frame """ - return self.joints_jacobian(frame, joint_positions) + if ( + self.frame_velocity_representation + == Representations.BODY_FIXED_REPRESENTATION + ): + return self.joints_jacobian(frame, joint_positions) + elif self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: + eye = self.math.factory.eye(4) + B_H_L = self.forward_kinematics(frame, eye, joint_positions) + LI_X_L = self.math.adjoint_mixed(B_H_L) + return LI_X_L @ self.joints_jacobian(frame, joint_positions) + else: + raise NotImplementedError( + "Only BODY_FIXED_REPRESENTATION and MIXED_REPRESENTATION are implemented" + ) def jacobian_dot( self, From 797a2f281fd35743c33e240633cd8f0eb5450930 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 11 Oct 2023 16:08:48 +0200 Subject: [PATCH 21/39] Add jacobian_dot to the casadi interface --- src/adam/casadi/computations.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index 79e7ddc7..0caca37d 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -111,6 +111,22 @@ def relative_jacobian_fun(self, frame: str) -> cs.Function: J = self.rbdalgos.relative_jacobian(frame, s) return cs.Function("J", [s], [J.array], self.f_opts) + def jacobian_dot_fun(self, frame: str) -> cs.Function: + """Returns the Jacobian derivative relative to the specified frame + + Args: + frame (str): The frame to which the jacobian will be computed + + Returns: + J_dot (casADi function): The Jacobian derivative relative to the frame + """ + T_b = cs.SX.sym("T_b", 4, 4) + s = cs.SX.sym("s", self.NDoF) + v_b = cs.SX.sym("v_b", 6) + s_dot = cs.SX.sym("s_dot", self.NDoF) + J_dot = self.rbdalgos.jacobian_dot(frame, T_b, s, v_b, s_dot) + return cs.Function("J_dot", [T_b, s, v_b, s_dot], [J_dot.array], self.f_opts) + def CoM_position_fun(self) -> cs.Function: """Returns the CoM positon From 65cf80535e3bbcab0802557754c27b3d98c0cfb3 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 11 Oct 2023 16:15:01 +0200 Subject: [PATCH 22/39] Remove useless folder --- src/adam/parametric/computations.py | 174 ---------------------------- 1 file changed, 174 deletions(-) delete mode 100644 src/adam/parametric/computations.py diff --git a/src/adam/parametric/computations.py b/src/adam/parametric/computations.py deleted file mode 100644 index 50b7ebe3..00000000 --- a/src/adam/parametric/computations.py +++ /dev/null @@ -1,174 +0,0 @@ -# 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 casadi as cs -import numpy as np - -from adam.casadi.casadi_like import SpatialMath -from adam.core import RBDAlgorithms -from adam.model import Model -from adam.parametric import ParametricModelFactory - - -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. - """ - - def __init__( - self, - urdfstring: str, - joints_name_list: list, - root_link: str = "root_link", - 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: - """ - 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'. - """ - factory = URDFModelFactory(urdfstring, SpatialMath()) - model = Model.build(factory=factory, joints_name_list=joints_name_list) - self.rbdalgos = RBDAlgorithms(model=model) - self.NDoF = self.rbdalgos.NDoF - self.g = gravity - self.f_opts = f_opts - - def mass_matrix_fun(self) -> cs.Function: - """Returns the Mass Matrix functions computed the CRBA - - Returns: - M (casADi function): Mass Matrix - """ - T_b = cs.SX.sym("T_b", 4, 4) - s = cs.SX.sym("s", self.NDoF) - [M, _] = self.rbdalgos.crba(T_b, s) - return cs.Function("M", [T_b, s], [M.array], self.f_opts) - - def centroidal_momentum_matrix_fun(self) -> cs.Function: - """Returns the Centroidal Momentum Matrix functions computed the CRBA - - Returns: - Jcc (casADi function): Centroidal Momentum matrix - """ - T_b = cs.SX.sym("T_b", 4, 4) - s = cs.SX.sym("s", self.NDoF) - [_, Jcm] = self.rbdalgos.crba(T_b, s) - return cs.Function("Jcm", [T_b, s], [Jcm.array], self.f_opts) - - def forward_kinematics_fun(self, frame: str) -> 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 - """ - s = cs.SX.sym("s", self.NDoF) - T_b = cs.SX.sym("T_b", 4, 4) - T_fk = self.rbdalgos.forward_kinematics(frame, T_b, s) - return cs.Function("T_fk", [T_b, s], [T_fk.array], self.f_opts) - - def jacobian_fun(self, frame: str) -> cs.Function: - """Returns the Jacobian relative to the specified frame - - Args: - frame (str): The frame to which the jacobian will be computed - - Returns: - J_tot (casADi function): The Jacobian relative to the frame - """ - s = cs.SX.sym("s", self.NDoF) - T_b = cs.SX.sym("T_b", 4, 4) - J_tot = self.rbdalgos.jacobian(frame, T_b, s) - return cs.Function("J_tot", [T_b, s], [J_tot.array], self.f_opts) - - def relative_jacobian_fun(self, frame: str) -> cs.Function: - """Returns the Jacobian between the root link and a specified frame frames - - Args: - frame (str): The tip of the chain - - Returns: - J (casADi function): The Jacobian between the root and the frame - """ - s = cs.SX.sym("s", self.NDoF) - J = self.rbdalgos.relative_jacobian(frame, s) - return cs.Function("J", [s], [J.array], self.f_opts) - - def CoM_position_fun(self) -> cs.Function: - """Returns the CoM positon - - Returns: - com (casADi function): The CoM position - """ - s = cs.SX.sym("s", self.NDoF) - T_b = cs.SX.sym("T_b", 4, 4) - com_pos = self.rbdalgos.CoM_position(T_b, s) - return cs.Function("CoM_pos", [T_b, s], [com_pos.array], self.f_opts) - - def bias_force_fun(self) -> cs.Function: - """Returns the bias force of the floating-base dynamics equation, - using a reduced RNEA (no acceleration and external forces) - - Returns: - h (casADi function): the bias force - """ - T_b = cs.SX.sym("T_b", 4, 4) - s = cs.SX.sym("s", self.NDoF) - v_b = cs.SX.sym("v_b", 6) - s_dot = cs.SX.sym("s_dot", self.NDoF) - h = self.rbdalgos.rnea(T_b, s, v_b, s_dot, self.g) - return cs.Function("h", [T_b, s, v_b, s_dot], [h.array], self.f_opts) - - def coriolis_term_fun(self) -> cs.Function: - """Returns the coriolis term of the floating-base dynamics equation, - using a reduced RNEA (no acceleration and external forces) - - Returns: - C (casADi function): the Coriolis term - """ - T_b = cs.SX.sym("T_b", 4, 4) - q = cs.SX.sym("q", self.NDoF) - v_b = cs.SX.sym("v_b", 6) - q_dot = cs.SX.sym("q_dot", self.NDoF) - # set in the bias force computation the gravity term to zero - C = self.rbdalgos.rnea(T_b, q, v_b, q_dot, np.zeros(6)) - return cs.Function("C", [T_b, q, v_b, q_dot], [C.array], self.f_opts) - - def gravity_term_fun(self) -> cs.Function: - """Returns the gravity term of the floating-base dynamics equation, - using a reduced RNEA (no acceleration and external forces) - - Returns: - G (casADi function): the gravity term - """ - T_b = cs.SX.sym("T_b", 4, 4) - q = cs.SX.sym("q", self.NDoF) - # set in the bias force computation the velocity to zero - G = self.rbdalgos.rnea(T_b, q, np.zeros(6), np.zeros(self.NDoF), self.g) - return cs.Function("G", [T_b, q], [G.array], 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) - - def get_total_mass(self): - """Returns the total mass of the robot - - Returns: - mass: The total mass - """ - return self.rbdalgos.get_total_mass() From 916854d1861609f07e802749f6f9008a5db7848f Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 11 Oct 2023 16:15:17 +0200 Subject: [PATCH 23/39] Remove redundant tests --- tests/CasADi_computations_body.py | 208 ------------------------------ tests/NumPy_computations_body.py | 206 ----------------------------- 2 files changed, 414 deletions(-) delete mode 100644 tests/CasADi_computations_body.py delete mode 100644 tests/NumPy_computations_body.py diff --git a/tests/CasADi_computations_body.py b/tests/CasADi_computations_body.py deleted file mode 100644 index 92473bae..00000000 --- a/tests/CasADi_computations_body.py +++ /dev/null @@ -1,208 +0,0 @@ -# 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 - -import adam -from adam.casadi import KinDynComputations -from adam.geometry import utils - -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(adam.Representations.BODY_FIXED_REPRESENTATION) -robot_iDyn = idyntree.ModelLoader() -robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) - -kinDyn = idyntree.KinDynComputations() -kinDyn.loadRobotModel(robot_iDyn.model()) -kinDyn.setFloatingBase(root_link) -# kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) -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 - -# set iDynTree kinDyn -H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) -vb = idyntree.Twist.Zero() -[vb.setVal(i, base_vel[i]) for i in range(6)] - -s = idyntree.VectorDynSize(n_dofs) -[s.setVal(i, joints_val[i]) for i in range(n_dofs)] -s_dot = idyntree.VectorDynSize(n_dofs) -[s_dot.setVal(i, joints_dot_val[i]) for i in range(n_dofs)] - -g = idyntree.Vector3() -g.zero() -g.setVal(2, -9.80665) -kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) -# set ADAM -H_b = utils.H_from_Pos_RPY(xyz, rpy) -vb_ = base_vel -s_ = joints_val -s_dot_ = joints_dot_val - - -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, s_)) - assert mass_test - 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, s_)) - assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - com_f = comp.CoM_position_fun() - CoM_cs = cs.DM(com_f(H_b, s_)) - CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - assert CoM_cs - 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, s_)) - assert iDynNumpyJ_ - J_test == 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, s_)) - assert iDynNumpyJ_ - J_test == 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, s_)) - 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) - - -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, s_)) - 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) - - -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, s_, vb_, s_dot_)) - assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - g0 = idyntree.Vector3() - g0.zero() - kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, 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, s_, vb_, s_dot_)) - assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) - - -def test_gravity_term(): - vb0 = idyntree.Twist.Zero() - s_dot0 = idyntree.VectorDynSize(n_dofs) - kinDyn.setRobotState(H_b_idyn, s, vb0, s_dot0, 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, s_)) - assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/NumPy_computations_body.py b/tests/NumPy_computations_body.py deleted file mode 100644 index d0e3fe4f..00000000 --- a/tests/NumPy_computations_body.py +++ /dev/null @@ -1,206 +0,0 @@ -# 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 icub_models -import idyntree.swig as idyntree -import numpy as np -import pytest - -from adam import Representations -from adam.geometry import utils -from adam.numpy import KinDynComputations - -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.rbdalgos.set_frame_velocity_representation( - Representations.BODY_FIXED_REPRESENTATION -) - -robot_iDyn = idyntree.ModelLoader() -robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) -kinDyn = idyntree.KinDynComputations() -kinDyn.loadRobotModel(robot_iDyn.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 - -# set iDynTree kinDyn -H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) -vb = idyntree.Twist.Zero() -[vb.setVal(i, base_vel[i]) for i in range(6)] - -s = idyntree.VectorDynSize(n_dofs) -[s.setVal(i, joints_val[i]) for i in range(n_dofs)] -s_dot = idyntree.VectorDynSize(n_dofs) -[s_dot.setVal(i, joints_dot_val[i]) for i in range(n_dofs)] - -g = idyntree.Vector3() -g.zero() -g.setVal(2, -9.80665) -kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) -# set ADAM -H_b = utils.H_from_Pos_RPY(xyz, rpy) -vb_ = base_vel -s_ = joints_val -s_dot_ = joints_dot_val - - -def test_mass_matrix(): - mass_mx = idyntree.MatrixDynSize() - kinDyn.getFreeFloatingMassMatrix(mass_mx) - mass_mxNumpy = mass_mx.toNumPy() - mass_test = comp.mass_matrix(H_b, s_) - assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CMM(): - cmm_idyntree = idyntree.MatrixDynSize() - kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) - cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) - assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, s_) - CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - assert CoM_test - 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(): - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("l_sole", H_b, s_) - assert iDynNumpyJ_ - J_test == 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_test = comp.jacobian("head", H_b, s_) - assert iDynNumpyJ_ - J_test == 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() - H_test = comp.forward_kinematics("l_sole", H_b, s_) - 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) - - -def test_fk_non_actuated(): - H_idyntree = kinDyn.getWorldTransform("head") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("head", H_b, s_) - 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) - - -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_test = comp.bias_force(H_b, s_, vb_, s_dot_) - assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - g0 = idyntree.Vector3() - g0.zero() - kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, 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_test = comp.coriolis_term(H_b, s_, vb_, s_dot_) - assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) - - -# def test_gravity_term(): -kinDyn2 = idyntree.KinDynComputations() -kinDyn2.loadRobotModel(robot_iDyn.model()) -kinDyn2.setFloatingBase(root_link) -kinDyn2.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) -vb0 = idyntree.Twist.Zero() -s_dot0 = idyntree.VectorDynSize(n_dofs) -kinDyn2.setRobotState(H_b_idyn, s, vb0, s_dot0, g) -G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) -assert kinDyn2.generalizedBiasForces(G_iDyn) -G_iDyn_np = np.concatenate( - (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) -) -G_test = comp.gravity_term(H_b, s_) - -print("idyn", G_iDyn_np) -print("test", G_test) - -assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) From 3f7e4342e242e60263d95c5a12cfbbd611cb3f4b Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 11 Oct 2023 16:23:04 +0200 Subject: [PATCH 24/39] Add j_dot functions --- src/adam/jax/computations.py | 24 ++++++++++++++++++++++++ src/adam/pytorch/computations.py | 24 ++++++++++++++++++++++++ 2 files changed, 48 insertions(+) diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index 0281e43c..87e2dae4 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -87,6 +87,30 @@ def relative_jacobian(self, frame: str, joint_positions: jnp.array): """ return self.rbdalgos.relative_jacobian(frame, joint_positions).array + def jacobian_dot( + self, + frame: str, + base_transform: jnp.array, + joint_positions: jnp.array, + base_velocity: jnp.array, + joint_velocities: jnp.array, + ) -> jnp.array: + """Returns the Jacobian derivative relative to the specified frame + + Args: + frame (str): The frame to which the jacobian will be computed + base_transform (jnp.array): The homogenous transform from base to world frame + joint_positions (jnp.array): The joints position + base_velocity (jnp.array): The base velocity in mixed representation + joint_velocities (jnp.array): The joint velocities + + Returns: + Jdot (jnp.array): The Jacobian derivative relative to the frame + """ + return self.rbdalgos.jacobian_dot( + frame, base_transform, joint_positions, base_velocity, joint_velocities + ).array + def forward_kinematics( self, frame: str, base_transform: jnp.array, joint_positions: jnp.array ): diff --git a/src/adam/pytorch/computations.py b/src/adam/pytorch/computations.py index 0e6762b4..2fcefd1d 100644 --- a/src/adam/pytorch/computations.py +++ b/src/adam/pytorch/computations.py @@ -122,6 +122,30 @@ def relative_jacobian(self, frame, joint_positions: torch.Tensor) -> torch.Tenso """ return self.rbdalgos.relative_jacobian(frame, joint_positions).array + def jacobian_dot( + self, + frame: str, + base_transform: torch.Tensor, + joint_positions: torch.Tensor, + base_velocity: torch.Tensor, + joint_velocities: torch.Tensor, + ) -> torch.Tensor: + """Returns the Jacobian derivative relative to the specified frame + + Args: + frame (str): The frame to which the jacobian will be computed + base_transform (torch.Tensor): The homogenous transform from base to world frame + joint_positions (torch.Tensor): The joints position + base_velocity (torch.Tensor): The base velocity in mixed representation + joint_velocities (torch.Tensor): The joint velocities + + Returns: + Jdot (torch.Tensor): The Jacobian derivative relative to the frame + """ + return self.rbdalgos.jacobian_dot( + frame, base_transform, joint_positions, base_velocity, joint_velocities + ).array + def CoM_position( self, base_transform: torch.Tensor, joint_positions: torch.Tensor ) -> torch.Tensor: From ea6774725c60416a1ab700fbfa74af3453c4479f Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 11 Oct 2023 16:23:47 +0200 Subject: [PATCH 25/39] Add tests --- tests/body_fixed/test_CasADi_body_fixed.py | 8 +++++ tests/body_fixed/test_Jax_body_fixed.py | 8 +++++ tests/body_fixed/test_NumPy_body_fixed.py | 8 +++++ tests/body_fixed/test_pytorch_body_fixed.py | 8 +++++ tests/mixed/test_CasADi_mixed.py | 33 ++++++++++++++++----- tests/mixed/test_Jax_mixed.py | 8 +++++ tests/mixed/test_NumPy_mixed.py | 9 ++++++ tests/mixed/test_pytorch_mixed.py | 8 +++++ 8 files changed, 82 insertions(+), 8 deletions(-) diff --git a/tests/body_fixed/test_CasADi_body_fixed.py b/tests/body_fixed/test_CasADi_body_fixed.py index 39dda4b2..4eb17654 100644 --- a/tests/body_fixed/test_CasADi_body_fixed.py +++ b/tests/body_fixed/test_CasADi_body_fixed.py @@ -149,6 +149,14 @@ def test_jacobian_non_actuated(): assert iDynNumpyJ_ - J_test == 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, s_, vb_, s_dot_) @ np.concatenate((vb_, s_dot_)) + assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) + + def test_fk(): H_idyntree = kinDyn.getWorldTransform("l_sole") p_idy2np = H_idyntree.getPosition().toNumPy() diff --git a/tests/body_fixed/test_Jax_body_fixed.py b/tests/body_fixed/test_Jax_body_fixed.py index f60394fe..5221f19b 100644 --- a/tests/body_fixed/test_Jax_body_fixed.py +++ b/tests/body_fixed/test_Jax_body_fixed.py @@ -146,6 +146,14 @@ def test_jacobian_non_actuated(): assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-5) +def test_jacobian_dot(): + J_dot = comp.jacobian_dot("l_sole", H_b, s_, vb_, s_dot_) + Jdotnu = kinDyn.getFrameBiasAcc("l_sole") + Jdot_nu = Jdotnu.toNumPy() + J_dot_nu_test = J_dot @ np.concatenate((vb_, s_dot_)) + assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) + + def test_fk(): H_idyntree = kinDyn.getWorldTransform("l_sole") p_idy2np = H_idyntree.getPosition().toNumPy() diff --git a/tests/body_fixed/test_NumPy_body_fixed.py b/tests/body_fixed/test_NumPy_body_fixed.py index 1a8d4534..1e1ae465 100644 --- a/tests/body_fixed/test_NumPy_body_fixed.py +++ b/tests/body_fixed/test_NumPy_body_fixed.py @@ -143,6 +143,14 @@ def test_jacobian_non_actuated(): assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) +def test_jacobian_dot(): + J_dot = comp.jacobian_dot("l_sole", H_b, s_, vb_, s_dot_) + Jdotnu = kinDyn.getFrameBiasAcc("l_sole") + Jdot_nu = Jdotnu.toNumPy() + J_dot_nu_test = J_dot @ np.concatenate((vb_, s_dot_)) + assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) + + def test_fk(): H_idyntree = kinDyn.getWorldTransform("l_sole") p_idy2np = H_idyntree.getPosition().toNumPy() diff --git a/tests/body_fixed/test_pytorch_body_fixed.py b/tests/body_fixed/test_pytorch_body_fixed.py index f19a1fee..a1bf3c4b 100644 --- a/tests/body_fixed/test_pytorch_body_fixed.py +++ b/tests/body_fixed/test_pytorch_body_fixed.py @@ -145,6 +145,14 @@ def test_jacobian_non_actuated(): assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) +def test_jacobian_dot(): + J_dot = comp.jacobian_dot("l_sole", H_b, s_, vb_, s_dot_) + Jdotnu = kinDyn.getFrameBiasAcc("l_sole") + Jdot_nu = Jdotnu.toNumPy() + J_dot_nu_test = J_dot @ np.concatenate((vb_, s_dot_)) + assert Jdot_nu - np.array(J_dot_nu_test) == pytest.approx(0.0, abs=1e-5) + + def test_fk(): H_idyntree = kinDyn.getWorldTransform("l_sole") p_idy2np = H_idyntree.getPosition().toNumPy() diff --git a/tests/mixed/test_CasADi_mixed.py b/tests/mixed/test_CasADi_mixed.py index 445f16b4..075e92f5 100644 --- a/tests/mixed/test_CasADi_mixed.py +++ b/tests/mixed/test_CasADi_mixed.py @@ -12,6 +12,7 @@ from adam.casadi import KinDynComputations from adam.geometry import utils +from adam import Representations np.random.seed(42) @@ -59,6 +60,7 @@ def H_from_Pos_RPY_idyn(xyz, rpy): root_link = "root_link" comp = KinDynComputations(model_path, joints_name_list, root_link) +comp.set_frame_velocity_representation(Representations.MIXED_REPRESENTATION) robot_iDyn = idyntree.ModelLoader() robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) @@ -146,6 +148,15 @@ def test_jacobian_non_actuated(): assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) +def test_jacobian_dot(): + J_dot_fun = comp.jacobian_dot_fun("l_sole") + J_dot = J_dot_fun(H_b, s_, vb_, s_dot_) + Jdotnu = kinDyn.getFrameBiasAcc("l_sole") + Jdot_nu = Jdotnu.toNumPy() + J_dot_nu_test = J_dot @ cs.vertcat(vb_, s_dot_) + assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) + + def test_fk(): H_idyntree = kinDyn.getWorldTransform("l_sole") p_idy2np = H_idyntree.getPosition().toNumPy() @@ -205,11 +216,17 @@ def test_gravity_term(): assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) -def test_relative_jacobian(): - kinDyn.setRobotState(H_from_Pos_RPY_idyn(np.zeros(3), np.zeros(3)), s, vb, s_dot, g) - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] - J = comp.relative_jacobian_fun("l_sole") - J_test = cs.DM(J(s_)) - assert iDynNumpyRelativeJ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) +# def test_relative_jacobian(): +kinDyn.setRobotState(H_from_Pos_RPY_idyn(np.zeros(3), np.zeros(3)), s, vb, s_dot, g) +iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) +kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) +iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] +J = comp.relative_jacobian_fun("l_sole") +# J = comp.jacobian_fun("l_sole") +# J_test = cs.DM(J(np.eye(4), s_))[:, 6:] +J_test = cs.DM(J(s_)) + +print("iDynNumpyRelativeJ", iDynNumpyRelativeJ[0, :]) +print("J_test", J_test[0, :]) + +assert iDynNumpyRelativeJ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_Jax_mixed.py b/tests/mixed/test_Jax_mixed.py index c5e43260..87343caa 100644 --- a/tests/mixed/test_Jax_mixed.py +++ b/tests/mixed/test_Jax_mixed.py @@ -143,6 +143,14 @@ def test_jacobian_non_actuated(): assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-5) +def test_jacobian_dot(): + J_dot = comp.jacobian_dot("l_sole", H_b, s_, vb_, s_dot_) + Jdotnu = kinDyn.getFrameBiasAcc("l_sole") + Jdot_nu = Jdotnu.toNumPy() + J_dot_nu_test = J_dot @ np.concatenate((vb_, s_dot_)) + assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) + + def test_fk(): H_idyntree = kinDyn.getWorldTransform("l_sole") p_idy2np = H_idyntree.getPosition().toNumPy() diff --git a/tests/mixed/test_NumPy_mixed.py b/tests/mixed/test_NumPy_mixed.py index 39cbdefb..32f0912e 100644 --- a/tests/mixed/test_NumPy_mixed.py +++ b/tests/mixed/test_NumPy_mixed.py @@ -9,6 +9,7 @@ import numpy as np import pytest +from adam import Representations from adam.geometry import utils from adam.numpy import KinDynComputations @@ -140,6 +141,14 @@ def test_jacobian_non_actuated(): assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) +def test_jacobian_dot(): + J_dot = comp.jacobian_dot("l_sole", H_b, s_, vb_, s_dot_) + Jdotnu = kinDyn.getFrameBiasAcc("l_sole") + Jdot_nu = Jdotnu.toNumPy() + J_dot_nu_test = J_dot @ np.concatenate((vb_, s_dot_)) + assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) + + def test_fk(): H_idyntree = kinDyn.getWorldTransform("l_sole") p_idy2np = H_idyntree.getPosition().toNumPy() diff --git a/tests/mixed/test_pytorch_mixed.py b/tests/mixed/test_pytorch_mixed.py index e4dc7475..bc07b098 100644 --- a/tests/mixed/test_pytorch_mixed.py +++ b/tests/mixed/test_pytorch_mixed.py @@ -142,6 +142,14 @@ def test_jacobian_non_actuated(): assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) +def test_jacobian_dot(): + J_dot = comp.jacobian_dot("l_sole", H_b, s_, vb_, s_dot_) + Jdotnu = kinDyn.getFrameBiasAcc("l_sole") + Jdot_nu = Jdotnu.toNumPy() + J_dot_nu_test = J_dot @ np.concatenate((vb_, s_dot_)) + assert Jdot_nu - np.array(J_dot_nu_test) == pytest.approx(0.0, abs=1e-5) + + def test_fk(): H_idyntree = kinDyn.getWorldTransform("l_sole") p_idy2np = H_idyntree.getPosition().toNumPy() From 059a35200ce6b606464e8b5a320e3bb7f1e1bd60 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 11 Oct 2023 16:35:54 +0200 Subject: [PATCH 26/39] Fix function test --- tests/body_fixed/test_CasADi_body_fixed.py | 2 +- tests/mixed/test_CasADi_mixed.py | 22 ++++++++-------------- 2 files changed, 9 insertions(+), 15 deletions(-) diff --git a/tests/body_fixed/test_CasADi_body_fixed.py b/tests/body_fixed/test_CasADi_body_fixed.py index 4eb17654..388a3f21 100644 --- a/tests/body_fixed/test_CasADi_body_fixed.py +++ b/tests/body_fixed/test_CasADi_body_fixed.py @@ -224,4 +224,4 @@ def test_relative_jacobian(): iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] J_fun = comp.relative_jacobian_fun("l_sole") J_test = cs.DM(J_fun(s_)) - assert iDynNumpyRelativeJ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) + assert iDynNumpyRelativeJ - J_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_CasADi_mixed.py b/tests/mixed/test_CasADi_mixed.py index 075e92f5..64adbeb2 100644 --- a/tests/mixed/test_CasADi_mixed.py +++ b/tests/mixed/test_CasADi_mixed.py @@ -216,17 +216,11 @@ def test_gravity_term(): assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) -# def test_relative_jacobian(): -kinDyn.setRobotState(H_from_Pos_RPY_idyn(np.zeros(3), np.zeros(3)), s, vb, s_dot, g) -iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) -kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) -iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] -J = comp.relative_jacobian_fun("l_sole") -# J = comp.jacobian_fun("l_sole") -# J_test = cs.DM(J(np.eye(4), s_))[:, 6:] -J_test = cs.DM(J(s_)) - -print("iDynNumpyRelativeJ", iDynNumpyRelativeJ[0, :]) -print("J_test", J_test[0, :]) - -assert iDynNumpyRelativeJ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) +def test_relative_jacobian(): + kinDyn.setRobotState(H_from_Pos_RPY_idyn(np.zeros(3), np.zeros(3)), s, vb, s_dot, 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(s_)) + assert iDynNumpyRelativeJ - J_test == pytest.approx(0.0, abs=1e-4) From 30b01961d40f4dd4e0a0665f223aa51f937f4824 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 11 Oct 2023 19:54:52 +0200 Subject: [PATCH 27/39] Change some name arguments and add the possibility to call functions without having casadi funcitions --- src/adam/casadi/computations.py | 316 ++++++++++++++++++++++++++------ 1 file changed, 264 insertions(+), 52 deletions(-) diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index 0caca37d..24e058ba 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -4,6 +4,7 @@ import casadi as cs import numpy as np +from typing import Union from adam.casadi.casadi_like import SpatialMath from adam.core import RBDAlgorithms @@ -54,10 +55,12 @@ def mass_matrix_fun(self) -> cs.Function: Returns: M (casADi function): Mass Matrix """ - T_b = cs.SX.sym("T_b", 4, 4) - s = cs.SX.sym("s", self.NDoF) - [M, _] = self.rbdalgos.crba(T_b, s) - return cs.Function("M", [T_b, s], [M.array], self.f_opts) + base_transform = cs.SX.sym("H", 4, 4) + joint_positions = cs.SX.sym("s", self.NDoF) + [M, _] = self.rbdalgos.crba(base_transform, joint_positions) + return cs.Function( + "M", [base_transform, joint_positions], [M.array], self.f_opts + ) def centroidal_momentum_matrix_fun(self) -> cs.Function: """Returns the Centroidal Momentum Matrix functions computed the CRBA @@ -65,10 +68,12 @@ def centroidal_momentum_matrix_fun(self) -> cs.Function: Returns: Jcc (casADi function): Centroidal Momentum matrix """ - T_b = cs.SX.sym("T_b", 4, 4) - s = cs.SX.sym("s", self.NDoF) - [_, Jcm] = self.rbdalgos.crba(T_b, s) - return cs.Function("Jcm", [T_b, s], [Jcm.array], self.f_opts) + base_transform = cs.SX.sym("H", 4, 4) + joint_positions = cs.SX.sym("s", self.NDoF) + [_, Jcm] = self.rbdalgos.crba(base_transform, joint_positions) + return cs.Function( + "Jcm", [base_transform, joint_positions], [Jcm.array], self.f_opts + ) def forward_kinematics_fun(self, frame: str) -> cs.Function: """Computes the forward kinematics relative to the specified frame @@ -79,10 +84,12 @@ def forward_kinematics_fun(self, frame: str) -> cs.Function: Returns: T_fk (casADi function): The fk represented as Homogenous transformation matrix """ - s = cs.SX.sym("s", self.NDoF) - T_b = cs.SX.sym("T_b", 4, 4) - T_fk = self.rbdalgos.forward_kinematics(frame, T_b, s) - return cs.Function("T_fk", [T_b, s], [T_fk.array], self.f_opts) + joint_positions = cs.SX.sym("s", self.NDoF) + base_transform = cs.SX.sym("H", 4, 4) + T_fk = self.rbdalgos.forward_kinematics(frame, base_transform, joint_positions) + return cs.Function( + "T_fk", [base_transform, joint_positions], [T_fk.array], self.f_opts + ) def jacobian_fun(self, frame: str) -> cs.Function: """Returns the Jacobian relative to the specified frame @@ -93,10 +100,12 @@ def jacobian_fun(self, frame: str) -> cs.Function: Returns: J_tot (casADi function): The Jacobian relative to the frame """ - s = cs.SX.sym("s", self.NDoF) - T_b = cs.SX.sym("T_b", 4, 4) - J_tot = self.rbdalgos.jacobian(frame, T_b, s) - return cs.Function("J_tot", [T_b, s], [J_tot.array], self.f_opts) + joint_positions = cs.SX.sym("s", self.NDoF) + base_transform = cs.SX.sym("H", 4, 4) + J_tot = self.rbdalgos.jacobian(frame, base_transform, joint_positions) + return cs.Function( + "J_tot", [base_transform, joint_positions], [J_tot.array], self.f_opts + ) def relative_jacobian_fun(self, frame: str) -> cs.Function: """Returns the Jacobian between the root link and a specified frame frames @@ -107,9 +116,9 @@ def relative_jacobian_fun(self, frame: str) -> cs.Function: Returns: J (casADi function): The Jacobian between the root and the frame """ - s = cs.SX.sym("s", self.NDoF) - J = self.rbdalgos.relative_jacobian(frame, s) - return cs.Function("J", [s], [J.array], self.f_opts) + joint_positions = cs.SX.sym("s", self.NDoF) + J = self.rbdalgos.relative_jacobian(frame, joint_positions) + return cs.Function("J", [joint_positions], [J.array], self.f_opts) def jacobian_dot_fun(self, frame: str) -> cs.Function: """Returns the Jacobian derivative relative to the specified frame @@ -120,12 +129,19 @@ def jacobian_dot_fun(self, frame: str) -> cs.Function: Returns: J_dot (casADi function): The Jacobian derivative relative to the frame """ - T_b = cs.SX.sym("T_b", 4, 4) - s = cs.SX.sym("s", self.NDoF) - v_b = cs.SX.sym("v_b", 6) - s_dot = cs.SX.sym("s_dot", self.NDoF) - J_dot = self.rbdalgos.jacobian_dot(frame, T_b, s, v_b, s_dot) - return cs.Function("J_dot", [T_b, s, v_b, s_dot], [J_dot.array], self.f_opts) + base_transform = cs.SX.sym("H", 4, 4) + joint_positions = cs.SX.sym("s", self.NDoF) + base_velocity = cs.SX.sym("v_b", 6) + joint_velocities = cs.SX.sym("s_dot", self.NDoF) + J_dot = self.rbdalgos.jacobian_dot( + frame, base_transform, joint_positions, base_velocity, joint_velocities + ) + return cs.Function( + "J_dot", + [base_transform, joint_positions, base_velocity, joint_velocities], + [J_dot.array], + self.f_opts, + ) def CoM_position_fun(self) -> cs.Function: """Returns the CoM positon @@ -133,10 +149,12 @@ def CoM_position_fun(self) -> cs.Function: Returns: com (casADi function): The CoM position """ - s = cs.SX.sym("s", self.NDoF) - T_b = cs.SX.sym("T_b", 4, 4) - com_pos = self.rbdalgos.CoM_position(T_b, s) - return cs.Function("CoM_pos", [T_b, s], [com_pos.array], self.f_opts) + joint_positions = cs.SX.sym("s", self.NDoF) + base_transform = cs.SX.sym("H", 4, 4) + com_pos = self.rbdalgos.CoM_position(base_transform, joint_positions) + return cs.Function( + "CoM_pos", [base_transform, joint_positions], [com_pos.array], self.f_opts + ) def bias_force_fun(self) -> cs.Function: """Returns the bias force of the floating-base dynamics equation, @@ -145,12 +163,19 @@ def bias_force_fun(self) -> cs.Function: Returns: h (casADi function): the bias force """ - T_b = cs.SX.sym("T_b", 4, 4) - s = cs.SX.sym("s", self.NDoF) - v_b = cs.SX.sym("v_b", 6) - s_dot = cs.SX.sym("s_dot", self.NDoF) - h = self.rbdalgos.rnea(T_b, s, v_b, s_dot, self.g) - return cs.Function("h", [T_b, s, v_b, s_dot], [h.array], self.f_opts) + base_transform = cs.SX.sym("H", 4, 4) + joint_positions = cs.SX.sym("s", self.NDoF) + base_velocity = cs.SX.sym("v_b", 6) + joint_velocities = cs.SX.sym("s_dot", self.NDoF) + h = self.rbdalgos.rnea( + base_transform, joint_positions, base_velocity, joint_velocities, self.g + ) + return cs.Function( + "h", + [base_transform, joint_positions, base_velocity, joint_velocities], + [h.array], + self.f_opts, + ) def coriolis_term_fun(self) -> cs.Function: """Returns the coriolis term of the floating-base dynamics equation, @@ -159,13 +184,24 @@ def coriolis_term_fun(self) -> cs.Function: Returns: C (casADi function): the Coriolis term """ - T_b = cs.SX.sym("T_b", 4, 4) - q = cs.SX.sym("q", self.NDoF) - v_b = cs.SX.sym("v_b", 6) - q_dot = cs.SX.sym("q_dot", self.NDoF) + base_transform = cs.SX.sym("H", 4, 4) + joint_positions = cs.SX.sym("s", self.NDoF) + base_velocity = cs.SX.sym("v_b", 6) + joint_velocities = cs.SX.sym("s_dot", self.NDoF) # set in the bias force computation the gravity term to zero - C = self.rbdalgos.rnea(T_b, q, v_b, q_dot, np.zeros(6)) - return cs.Function("C", [T_b, q, v_b, q_dot], [C.array], self.f_opts) + C = self.rbdalgos.rnea( + base_transform, + joint_positions, + base_velocity, + joint_velocities, + np.zeros(6), + ) + return cs.Function( + "C", + [base_transform, joint_positions, base_velocity, joint_velocities], + [C.array], + self.f_opts, + ) def gravity_term_fun(self) -> cs.Function: """Returns the gravity term of the floating-base dynamics equation, @@ -174,28 +210,204 @@ def gravity_term_fun(self) -> cs.Function: Returns: G (casADi function): the gravity term """ - T_b = cs.SX.sym("T_b", 4, 4) - q = cs.SX.sym("q", self.NDoF) + base_transform = cs.SX.sym("H", 4, 4) + joint_positions = cs.SX.sym("s", self.NDoF) # set in the bias force computation the velocity to zero - G = self.rbdalgos.rnea(T_b, q, np.zeros(6), np.zeros(self.NDoF), self.g) - return cs.Function("G", [T_b, q], [G.array], self.f_opts) + G = self.rbdalgos.rnea( + base_transform, joint_positions, np.zeros(6), np.zeros(self.NDoF), self.g + ) + return cs.Function( + "G", [base_transform, joint_positions], [G.array], self.f_opts + ) - def forward_kinematics(self, frame, T_b, s) -> cs.Function: + def get_total_mass(self) -> float: + """Returns the total mass of the robot + + Returns: + mass: The total mass + """ + return self.rbdalgos.get_total_mass() + + def mass_matrix( + self, base_transform: Union[cs.SX, cs.DM], joint_positions: Union[cs.SX, cs.DM] + ): + """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 + + Returns: + M (jax): Mass Matrix + """ + [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] + ): + """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 + + Returns: + Jcc (Union[cs.SX, cs.DM]): Centroidal Momentum matrix + """ + [_, Jcm] = self.rbdalgos.crba(base_transform, joint_positions) + return Jcm.array + + def relative_jacobian(self, frame: str, joint_positions: Union[cs.SX, cs.DM]): + """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 + + Returns: + J (Union[cs.SX, cs.DM]): The Jacobian between the root and the frame + """ + return self.rbdalgos.relative_jacobian(frame, joint_positions).array + + 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]: + """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 + + Returns: + Jdot (Union[cs.SX, cs.DM]): The Jacobian derivative relative to the frame + """ + return self.rbdalgos.jacobian_dot( + frame, base_transform, joint_positions, base_velocity, joint_velocities + ).array + + def forward_kinematics( + self, + frame: str, + base_transform: Union[cs.SX, cs.DM], + joint_positions: Union[cs.SX, cs.DM], + ): """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 Returns: - T_fk (casADi function): The fk represented as Homogenous transformation matrix + T_fk (Union[cs.SX, cs.DM]): The fk represented as Homogenous transformation matrix """ + return self.rbdalgos.forward_kinematics( + frame, base_transform, joint_positions + ).array - return self.rbdalgos.forward_kinematics(frame, T_b, s) + def jacobian(self, frame: str, base_transform, joint_positions): + """Returns the Jacobian relative to the specified frame - def get_total_mass(self) -> float: - """Returns the total mass of the robot + 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 + frame (str): The frame to which the jacobian will be computed Returns: - mass: The total mass + J_tot (Union[cs.SX, cs.DM]): The Jacobian relative to the frame """ - return self.rbdalgos.get_total_mass() + return self.rbdalgos.jacobian(frame, base_transform, joint_positions).array + + 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]: + """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 + + Returns: + h (Union[cs.SX, cs.DM]): the bias force + """ + return self.rbdalgos.rnea( + base_transform, joint_positions, base_velocity, joint_velocities, self.g + ).array + + 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]: + """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 + + Returns: + C (Union[cs.SX, cs.DM]): the Coriolis term + """ + return self.rbdalgos.rnea( + base_transform, + joint_positions, + base_velocity.reshape(6, 1), + joint_velocities, + 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]: + """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 + + Returns: + G (Union[cs.SX, cs.DM]): the gravity term + """ + return self.rbdalgos.rnea( + base_transform, + joint_positions, + np.zeros(6).reshape(6, 1), + np.zeros(self.NDoF), + 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]: + """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 + + Returns: + com (Union[cs.SX, cs.DM]): The CoM position + """ + return self.rbdalgos.CoM_position(base_transform, joint_positions).array From 84c62cdb286a4a18b22843858c768325b3a24416 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 11 Oct 2023 19:55:09 +0200 Subject: [PATCH 28/39] Update tests including new functions --- tests/body_fixed/test_CasADi_body_fixed.py | 32 ++++++++++++++++-- tests/mixed/test_CasADi_mixed.py | 38 +++++++++++++++++++--- 2 files changed, 63 insertions(+), 7 deletions(-) diff --git a/tests/body_fixed/test_CasADi_body_fixed.py b/tests/body_fixed/test_CasADi_body_fixed.py index 388a3f21..52052b1f 100644 --- a/tests/body_fixed/test_CasADi_body_fixed.py +++ b/tests/body_fixed/test_CasADi_body_fixed.py @@ -106,7 +106,9 @@ def test_mass_matrix(): kinDyn.getFreeFloatingMassMatrix(mass_mx) mass_mxNumpy = mass_mx.toNumPy() mass_test = cs.DM(M(H_b, s_)) + mass_test2 = cs.DM(comp.mass_matrix(H_b, s_)) 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(): @@ -115,14 +117,18 @@ def test_CMM(): kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) cmm_idyntreeNumpy = cmm_idyntree.toNumPy() Jcm_test = cs.DM(Jcm(H_b, s_)) + Jcm_test2 = cs.DM(comp.centroidal_momentum_matrix(H_b, s_)) 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_cs = cs.DM(com_f(H_b, s_)) + CoM_test = cs.DM(com_f(H_b, s_)) CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - assert CoM_cs - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) + CoM_test2 = cs.DM(comp.CoM_position(H_b, s_)) + 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(): @@ -137,7 +143,9 @@ def test_jacobian(): kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() J_test = cs.DM(J_tot(H_b, s_)) + J_test2 = cs.DM(comp.jacobian("l_sole", H_b, s_)) 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(): @@ -146,7 +154,9 @@ def test_jacobian_non_actuated(): kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() J_test = cs.DM(J_tot(H_b, s_)) + J_test2 = cs.DM(comp.jacobian("head", H_b, s_)) 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(): @@ -154,7 +164,11 @@ def test_jacobian_dot(): Jdotnu = kinDyn.getFrameBiasAcc("l_sole") Jdot_nu = Jdotnu.toNumPy() J_dot_nu_test = J_dot(H_b, s_, vb_, s_dot_) @ np.concatenate((vb_, s_dot_)) + J_dot_nu_test2 = cs.DM( + comp.jacobian_dot("l_sole", H_b, s_, vb_, s_dot_) + ) @ np.concatenate((vb_, s_dot_)) 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(): @@ -163,8 +177,11 @@ def test_fk(): R_idy2np = H_idyntree.getRotation().toNumPy() T = comp.forward_kinematics_fun("l_sole") H_test = cs.DM(T(H_b, s_)) + H_test2 = cs.DM(comp.forward_kinematics("l_sole", H_b, s_)) 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(): @@ -173,8 +190,11 @@ def test_fk_non_actuated(): R_idy2np = H_idyntree.getRotation().toNumPy() T = comp.forward_kinematics_fun("head") H_test = cs.DM(T(H_b, s_)) + H_test2 = cs.DM(comp.forward_kinematics("head", H_b, s_)) 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(): @@ -185,7 +205,9 @@ def test_bias_force(): ) h = comp.bias_force_fun() h_test = cs.DM(h(H_b, s_, vb_, s_dot_)) + h_test2 = cs.DM(comp.bias_force(H_b, s_, vb_, s_dot_)) 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(): @@ -199,7 +221,9 @@ def test_coriolis_term(): ) C = comp.coriolis_term_fun() C_test = cs.DM(C(H_b, s_, vb_, s_dot_)) + C_test2 = cs.DM(comp.coriolis_term(H_b, s_, vb_, s_dot_)) 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(): @@ -214,7 +238,9 @@ def test_gravity_term(): ) G = comp.gravity_term_fun() G_test = cs.DM(G(H_b, s_)) + G_test2 = cs.DM(comp.gravity_term(H_b, s_)) 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(): @@ -224,4 +250,6 @@ def test_relative_jacobian(): iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] J_fun = comp.relative_jacobian_fun("l_sole") J_test = cs.DM(J_fun(s_)) + J_test2 = cs.DM(comp.relative_jacobian("l_sole", s_)) assert iDynNumpyRelativeJ - J_test == pytest.approx(0.0, abs=1e-4) + assert iDynNumpyRelativeJ - J_test2 == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_CasADi_mixed.py b/tests/mixed/test_CasADi_mixed.py index 64adbeb2..4ad99bf5 100644 --- a/tests/mixed/test_CasADi_mixed.py +++ b/tests/mixed/test_CasADi_mixed.py @@ -105,7 +105,9 @@ def test_mass_matrix(): kinDyn.getFreeFloatingMassMatrix(mass_mx) mass_mxNumpy = mass_mx.toNumPy() mass_test = cs.DM(M(H_b, s_)) + mass_test2 = cs.DM(comp.mass_matrix(H_b, s_)) 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(): @@ -114,14 +116,18 @@ def test_CMM(): kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) cmm_idyntreeNumpy = cmm_idyntree.toNumPy() Jcm_test = cs.DM(Jcm(H_b, s_)) + Jcm_test2 = cs.DM(comp.centroidal_momentum_matrix(H_b, s_)) 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_cs = cs.DM(com_f(H_b, s_)) + CoM_test = cs.DM(com_f(H_b, s_)) CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - assert CoM_cs - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) + CoM_test2 = cs.DM(comp.CoM_position(H_b, s_)) + 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(): @@ -136,7 +142,9 @@ def test_jacobian(): kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() J_test = cs.DM(J_tot(H_b, s_)) + J_test2 = cs.DM(comp.jacobian("l_sole", H_b, s_)) 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,16 +153,21 @@ def test_jacobian_non_actuated(): kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() J_test = cs.DM(J_tot(H_b, s_)) + J_test2 = cs.DM(comp.jacobian("head", H_b, s_)) 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_fun = comp.jacobian_dot_fun("l_sole") - J_dot = J_dot_fun(H_b, s_, vb_, s_dot_) + J_dot = comp.jacobian_dot_fun("l_sole") Jdotnu = kinDyn.getFrameBiasAcc("l_sole") Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot @ cs.vertcat(vb_, s_dot_) + J_dot_nu_test = J_dot(H_b, s_, vb_, s_dot_) @ np.concatenate((vb_, s_dot_)) + J_dot_nu_test2 = cs.DM( + comp.jacobian_dot("l_sole", H_b, s_, vb_, s_dot_) + ) @ np.concatenate((vb_, s_dot_)) 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(): @@ -163,8 +176,11 @@ def test_fk(): R_idy2np = H_idyntree.getRotation().toNumPy() T = comp.forward_kinematics_fun("l_sole") H_test = cs.DM(T(H_b, s_)) + H_test2 = cs.DM(comp.forward_kinematics("l_sole", H_b, s_)) 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(): @@ -173,8 +189,11 @@ def test_fk_non_actuated(): R_idy2np = H_idyntree.getRotation().toNumPy() T = comp.forward_kinematics_fun("head") H_test = cs.DM(T(H_b, s_)) + H_test2 = cs.DM(comp.forward_kinematics("head", H_b, s_)) 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(): @@ -185,7 +204,9 @@ def test_bias_force(): ) h = comp.bias_force_fun() h_test = cs.DM(h(H_b, s_, vb_, s_dot_)) + h_test2 = cs.DM(comp.bias_force(H_b, s_, vb_, s_dot_)) 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(): @@ -199,12 +220,15 @@ def test_coriolis_term(): ) C = comp.coriolis_term_fun() C_test = cs.DM(C(H_b, s_, vb_, s_dot_)) + C_test2 = cs.DM(comp.coriolis_term(H_b, s_, vb_, s_dot_)) 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(): vb0 = idyntree.Twist.Zero() s_dot0 = idyntree.VectorDynSize(n_dofs) + kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) kinDyn.setRobotState(H_b_idyn, s, vb0, s_dot0, g) G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) assert kinDyn.generalizedBiasForces(G_iDyn) @@ -213,7 +237,9 @@ def test_gravity_term(): ) G = comp.gravity_term_fun() G_test = cs.DM(G(H_b, s_)) + G_test2 = cs.DM(comp.gravity_term(H_b, s_)) 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(): @@ -223,4 +249,6 @@ def test_relative_jacobian(): iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] J_fun = comp.relative_jacobian_fun("l_sole") J_test = cs.DM(J_fun(s_)) + J_test2 = cs.DM(comp.relative_jacobian("l_sole", s_)) assert iDynNumpyRelativeJ - J_test == pytest.approx(0.0, abs=1e-4) + assert iDynNumpyRelativeJ - J_test2 == pytest.approx(0.0, abs=1e-4) From beaa2c2d3ef4f2df9922b19af9618ecee3f0f587 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 11 Oct 2023 20:02:00 +0200 Subject: [PATCH 29/39] Use a different names for arguments --- src/adam/casadi/computations.py | 8 +++---- src/adam/jax/computations.py | 18 +++++++------- src/adam/numpy/computations.py | 2 +- src/adam/pytorch/computations.py | 41 ++++++++++++++++++-------------- 4 files changed, 37 insertions(+), 32 deletions(-) diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index 24e058ba..a365b129 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -82,13 +82,13 @@ def forward_kinematics_fun(self, frame: str) -> cs.Function: frame (str): The frame to which the fk will be computed Returns: - T_fk (casADi function): The fk represented as Homogenous transformation matrix + H (casADi function): The fk represented as Homogenous transformation matrix """ joint_positions = cs.SX.sym("s", self.NDoF) base_transform = cs.SX.sym("H", 4, 4) - T_fk = self.rbdalgos.forward_kinematics(frame, base_transform, joint_positions) + H = self.rbdalgos.forward_kinematics(frame, base_transform, joint_positions) return cs.Function( - "T_fk", [base_transform, joint_positions], [T_fk.array], self.f_opts + "H", [base_transform, joint_positions], [H.array], self.f_opts ) def jacobian_fun(self, frame: str) -> cs.Function: @@ -308,7 +308,7 @@ def forward_kinematics( joint_positions (Union[cs.SX, cs.DM]): The joints position Returns: - T_fk (Union[cs.SX, cs.DM]): The fk represented as Homogenous transformation matrix + H (Union[cs.SX, cs.DM]): The fk represented as Homogenous transformation matrix """ return self.rbdalgos.forward_kinematics( frame, base_transform, joint_positions diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index 87e2dae4..f88a1d1e 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -151,22 +151,22 @@ def bias_force( base_transform: jnp.array, joint_positions: jnp.array, base_velocity: jnp.array, - s_dot: jnp.array, + joint_velocities: jnp.array, ) -> jnp.array: - """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: base_transform (jnp.array): The homogenous transform from base to world frame joint_positions (jnp.array): The joints position base_velocity (jnp.array): The base velocity in mixed representation - s_dot (jnp.array): The joints velocity + joint_velocities (jnp.array): The joints velocity Returns: h (jnp.array): the bias force """ return self.rbdalgos.rnea( - base_transform, joint_positions, base_velocity, s_dot, self.g + base_transform, joint_positions, base_velocity, joint_velocities, self.g ).array.squeeze() def coriolis_term( @@ -174,16 +174,16 @@ def coriolis_term( base_transform: jnp.array, joint_positions: jnp.array, base_velocity: jnp.array, - s_dot: jnp.array, + joint_velocities: jnp.array, ) -> jnp.array: - """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: base_transform (jnp.array): The homogenous transform from base to world frame joint_positions (jnp.array): The joints position base_velocity (jnp.array): The base velocity in mixed representation - s_dot (jnp.array): The joints velocity + joint_velocities (jnp.array): The joints velocity Returns: C (jnp.array): the Coriolis term @@ -192,14 +192,14 @@ def coriolis_term( base_transform, joint_positions, base_velocity.reshape(6, 1), - s_dot, + joint_velocities, np.zeros(6), ).array.squeeze() def gravity_term( self, base_transform: jnp.array, joint_positions: jnp.array ) -> jnp.array: - """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: diff --git a/src/adam/numpy/computations.py b/src/adam/numpy/computations.py index bf0e8759..316c0218 100644 --- a/src/adam/numpy/computations.py +++ b/src/adam/numpy/computations.py @@ -86,7 +86,7 @@ def forward_kinematics( joint_positions (np.ndarray): The joints position Returns: - T_fk (np.ndarray): The fk represented as Homogenous transformation matrix + H (np.ndarray): The fk represented as Homogenous transformation matrix """ return self.rbdalgos.forward_kinematics( frame, base_transform, joint_positions diff --git a/src/adam/pytorch/computations.py b/src/adam/pytorch/computations.py index 2fcefd1d..c02d256f 100644 --- a/src/adam/pytorch/computations.py +++ b/src/adam/pytorch/computations.py @@ -47,56 +47,61 @@ def set_frame_velocity_representation( self.rbdalgos.set_frame_velocity_representation(representation) def mass_matrix( - self, base_transform: torch.Tensor, s: torch.Tensor + self, base_transform: torch.Tensor, joint_position: torch.Tensor ) -> torch.Tensor: """Returns the Mass Matrix functions computed the CRBA Args: base_transform (torch.tensor): The homogenous transform from base to world frame - s (torch.tensor): The joints position + joint_positions (torch.tensor): The joints position Returns: M (torch.tensor): Mass Matrix """ - [M, _] = self.rbdalgos.crba(base_transform, s) + [M, _] = self.rbdalgos.crba(base_transform, joint_position) return M.array def centroidal_momentum_matrix( - self, base_transform: torch.Tensor, s: torch.Tensor + self, base_transform: torch.Tensor, joint_position: torch.Tensor ) -> torch.Tensor: """Returns the Centroidal Momentum Matrix functions computed the CRBA Args: base_transform (torch.tensor): The homogenous transform from base to world frame - s (torch.tensor): The joints position + joint_positions (torch.tensor): The joints position Returns: Jcc (torch.tensor): Centroidal Momentum matrix """ - [_, Jcm] = self.rbdalgos.crba(base_transform, s) + [_, Jcm] = self.rbdalgos.crba(base_transform, joint_position) return Jcm.array def forward_kinematics( - self, frame, base_transform: torch.Tensor, s: torch.Tensor + self, frame, base_transform: torch.Tensor, joint_position: torch.Tensor ) -> torch.Tensor: """Computes the forward kinematics relative to the specified frame Args: frame (str): The frame to which the fk will be computed base_transform (torch.tensor): The homogenous transform from base to world frame - s (torch.tensor): The joints position + joint_positions (torch.tensor): The joints position Returns: - T_fk (torch.tensor): The fk represented as Homogenous transformation matrix + H (torch.tensor): The fk represented as Homogenous transformation matrix """ return ( self.rbdalgos.forward_kinematics( - frame, torch.FloatTensor(base_transform), torch.FloatTensor(s) + frame, + torch.FloatTensor(base_transform), + torch.FloatTensor(joint_position), ) ).array def jacobian( - self, frame: str, base_transform: torch.Tensor, joint_positions: torch.Tensor + self, + frame: str, + base_transform: torch.Tensor, + joint_positions: torch.Tensor, ) -> torch.Tensor: """Returns the Jacobian relative to the specified frame @@ -156,7 +161,7 @@ def CoM_position( joint_positions (torch.tensor): The joints position Returns: - com (torch.tensor): The CoM position + CoM (torch.tensor): The CoM position """ return self.rbdalgos.CoM_position( base_transform, joint_positions @@ -165,7 +170,7 @@ def CoM_position( def bias_force( self, base_transform: torch.Tensor, - s: torch.Tensor, + joint_positions: torch.Tensor, base_velocity: torch.Tensor, joint_velocities: torch.Tensor, ) -> torch.Tensor: @@ -174,7 +179,7 @@ def bias_force( Args: base_transform (torch.tensor): The homogenous transform from base to world frame - s (torch.tensor): The joints position + joint_positions (torch.tensor): The joints position base_velocity (torch.tensor): The base velocity in mixed representation joint_velocities (torch.tensor): The joints velocity @@ -183,7 +188,7 @@ def bias_force( """ return self.rbdalgos.rnea( base_transform, - s, + joint_positions, base_velocity.reshape(6, 1), joint_velocities, self.g, @@ -218,21 +223,21 @@ def coriolis_term( ).array.squeeze() def gravity_term( - self, base_transform: torch.Tensor, base_positions: torch.Tensor + self, base_transform: torch.Tensor, joint_positions: torch.Tensor ) -> torch.Tensor: """Returns the gravity term of the floating-base dynamics ejoint_positionsuation, using a reduced RNEA (no acceleration and external forces) Args: base_transform (torch.tensor): The homogenous transform from base to world frame - base_positions (torch.tensor): The joints position + joint_positions (torch.tensor): The joints positions Returns: G (torch.tensor): the gravity term """ return self.rbdalgos.rnea( base_transform, - base_positions, + joint_positions, torch.zeros(6).reshape(6, 1), torch.zeros(self.NDoF), torch.FloatTensor(self.g), From 626b211b62c33225d3b7376c4c9769c5896cf3cd Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 11 Oct 2023 20:02:25 +0200 Subject: [PATCH 30/39] Update readme --- README.md | 23 +++++++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 422445e8..f4399b13 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ **Automatic Differentiation for rigid-body-dynamics AlgorithMs** -ADAM implements a collection of algorithms for calculating rigid-body dynamics for **floating-base** robots, in _mixed representation_ (see [Traversaro's A Unified View of the Equations of Motion used for Control Design of Humanoid Robots](https://www.researchgate.net/publication/312200239_A_Unified_View_of_the_Equations_of_Motion_used_for_Control_Design_of_Humanoid_Robots)) using: +ADAM implements a collection of algorithms for calculating rigid-body dynamics for **floating-base** robots, in _mixed_ and _base fixed representations_, also called _left trivialized_ representation (see [Traversaro's A Unified View of the Equations of Motion used for Control Design of Humanoid Robots](https://www.researchgate.net/publication/312200239_A_Unified_View_of_the_Equations_of_Motion_used_for_Control_Design_of_Humanoid_Robots)) using: - [Jax](https://github.com/google/jax) - [CasADi](https://web.casadi.org/) @@ -43,7 +43,7 @@ They will be installed in the installation step! The installation can be done either using the Python provided by apt (on Debian-based distros) or via conda (on Linux and macOS). -### Installation with pip +### 🐍 Installation with pip Install `python3`, if not installed (in **Ubuntu 20.04**): @@ -99,7 +99,7 @@ cd ADAM pip install .[selected-interface] ``` -### Installation with conda +### 📦 Installation with conda #### Installation from conda-forge package @@ -109,7 +109,7 @@ mamba create -n adamenv -c conda-forge adam-robotics If you want to use `jax` or `pytorch`, just install the corresponding package as well. -#### Installation from repo +### 🔨 Installation from repo Install in a conda environment the required dependencies: @@ -154,6 +154,7 @@ Have also a look at te `tests` folder. ### Jax interface ```python +import adam from adam.jax import KinDynComputations import icub_models import numpy as np @@ -171,6 +172,10 @@ joints_name_list = [ # Specify the root link root_link = 'root_link' kinDyn = KinDynComputations(model_path, joints_name_list, root_link) +# choose the representation you want to use the body fixed representation +kinDyn.set_frame_velocity_representation(adam.Representations.BODY_FIXED_REPRESENTATION) +# or, if you want to use the mixed representation (that is the default) +kinDyn.set_frame_velocity_representation(adam.Representations.MIXED_REPRESENTATION) w_H_b = np.eye(4) joints = np.ones(len(joints_name_list)) M = kinDyn.mass_matrix(w_H_b, joints) @@ -180,6 +185,7 @@ print(M) ### CasADi interface ```python +import adam from adam.casadi import KinDynComputations import icub_models import numpy as np @@ -197,6 +203,10 @@ joints_name_list = [ # Specify the root link root_link = 'root_link' kinDyn = KinDynComputations(model_path, joints_name_list, root_link) +# choose the representation you want to use the body fixed representation +kinDyn.set_frame_velocity_representation(adam.Representations.BODY_FIXED_REPRESENTATION) +# or, if you want to use the mixed representation (that is the default) +kinDyn.set_frame_velocity_representation(adam.Representations.MIXED_REPRESENTATION) w_H_b = np.eye(4) joints = np.ones(len(joints_name_list)) M = kinDyn.mass_matrix_fun() @@ -206,6 +216,7 @@ print(M(w_H_b, joints)) ### PyTorch interface ```python +import adam from adam.pytorch import KinDynComputations import icub_models import numpy as np @@ -223,6 +234,10 @@ joints_name_list = [ # Specify the root link root_link = 'root_link' kinDyn = KinDynComputations(model_path, joints_name_list, root_link) +# choose the representation you want to use the body fixed representation +kinDyn.set_frame_velocity_representation(adam.Representations.BODY_FIXED_REPRESENTATION) +# or, if you want to use the mixed representation (that is the default) +kinDyn.set_frame_velocity_representation(adam.Representations.MIXED_REPRESENTATION) w_H_b = np.eye(4) joints = np.ones(len(joints_name_list)) M = kinDyn.mass_matrix(w_H_b, joints) From e19cc34b9cb3e8f351fe4f1eecbdaa6cb69a9a8d Mon Sep 17 00:00:00 2001 From: Giuseppe L'Erario Date: Thu, 12 Oct 2023 14:45:22 +0200 Subject: [PATCH 31/39] Replace Enum with IntEnum Co-authored-by: Filippo Luca Ferretti <102977828+flferretti@users.noreply.github.com> --- src/adam/core/constants.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/adam/core/constants.py b/src/adam/core/constants.py index fe4e4614..60c66146 100644 --- a/src/adam/core/constants.py +++ b/src/adam/core/constants.py @@ -1,6 +1,6 @@ -from enum import Enum +from enum import IntEnum -class Representations(Enum): - BODY_FIXED_REPRESENTATION = 0 - MIXED_REPRESENTATION = 1 +class Representations(IntEnum): + BODY_FIXED_REPRESENTATION = enum.auto() + MIXED_REPRESENTATION = enum.auto() From 99cb4063291a13e96e4f4bfa31d4e420cd684213 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 12 Oct 2023 14:48:48 +0200 Subject: [PATCH 32/39] Fix auto --- src/adam/core/constants.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/adam/core/constants.py b/src/adam/core/constants.py index 60c66146..44ef2d35 100644 --- a/src/adam/core/constants.py +++ b/src/adam/core/constants.py @@ -1,6 +1,6 @@ -from enum import IntEnum +from enum import IntEnum, auto class Representations(IntEnum): - BODY_FIXED_REPRESENTATION = enum.auto() - MIXED_REPRESENTATION = enum.auto() + BODY_FIXED_REPRESENTATION = auto() + MIXED_REPRESENTATION = auto() From 4baaa56b87d867e4f709674ff36820f9087d6751 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 12 Oct 2023 14:50:09 +0200 Subject: [PATCH 33/39] Replace abstractproperty --- src/adam/core/spatial_math.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py index 2c6bb7a4..d74bc6d0 100644 --- a/src/adam/core/spatial_math.py +++ b/src/adam/core/spatial_math.py @@ -56,7 +56,8 @@ def __setitem__(self, key, value): def __truediv__(self, other): pass - @abc.abstractproperty + @property + @abc.abstractmethod def T(self): """ Returns: Transpose of the array From 3f220822c3847c2c160d10e6202a8857c7b3ca01 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 12 Oct 2023 14:51:11 +0200 Subject: [PATCH 34/39] Remove useless import --- src/adam/jax/computations.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index f88a1d1e..fe9bc7e9 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -4,7 +4,6 @@ import jax.numpy as jnp import numpy as np -from jax import grad, jit, vmap from adam.core.constants import Representations from adam.core.rbd_algorithms import RBDAlgorithms From f91f478bf970376733e35ce0ebfd4e12c5c4bfa9 Mon Sep 17 00:00:00 2001 From: giulero Date: Sat, 14 Oct 2023 13:33:19 +0200 Subject: [PATCH 35/39] Update doc --- src/adam/casadi/computations.py | 4 ++-- src/adam/jax/computations.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index a365b129..1fb50e48 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -147,7 +147,7 @@ def CoM_position_fun(self) -> cs.Function: """Returns the CoM positon Returns: - com (casADi function): The CoM position + CoM (casADi function): The CoM position """ joint_positions = cs.SX.sym("s", self.NDoF) base_transform = cs.SX.sym("H", 4, 4) @@ -408,6 +408,6 @@ def CoM_position( joint_positions (Union[cs.SX, cs.DM]): The joints position Returns: - com (Union[cs.SX, cs.DM]): The CoM position + CoM (Union[cs.SX, cs.DM]): The CoM position """ return self.rbdalgos.CoM_position(base_transform, joint_positions).array diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index fe9bc7e9..766e8a81 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -121,7 +121,7 @@ def forward_kinematics( joint_positions (jnp.array): The joints position Returns: - T_fk (jnp.array): The fk represented as Homogenous transformation matrix + H (jnp.array): The fk represented as Homogenous transformation matrix """ return self.rbdalgos.forward_kinematics( frame, base_transform, joint_positions @@ -226,7 +226,7 @@ def CoM_position( joint_positions (jnp.array): The joints position Returns: - com (jnp.array): The CoM position + CoM (jnp.array): The CoM position """ return self.rbdalgos.CoM_position( base_transform, joint_positions From 810168d11820819b9e747659d222856b48655fe3 Mon Sep 17 00:00:00 2001 From: giulero Date: Sat, 14 Oct 2023 13:54:06 +0200 Subject: [PATCH 36/39] Cleanup casadi and numpy mixed tests --- tests/mixed/test_CasADi_mixed.py | 83 ++++++++++++++------------------ tests/mixed/test_NumPy_mixed.py | 55 ++++++++------------- 2 files changed, 54 insertions(+), 84 deletions(-) diff --git a/tests/mixed/test_CasADi_mixed.py b/tests/mixed/test_CasADi_mixed.py index 4ad99bf5..60c1c102 100644 --- a/tests/mixed/test_CasADi_mixed.py +++ b/tests/mixed/test_CasADi_mixed.py @@ -78,25 +78,9 @@ def H_from_Pos_RPY_idyn(xyz, rpy): joints_val = (np.random.rand(n_dofs) - 0.5) * 5 joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 -# set iDynTree kinDyn -H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) -vb = idyntree.Twist.Zero() -[vb.setVal(i, base_vel[i]) for i in range(6)] - -s = idyntree.VectorDynSize(n_dofs) -[s.setVal(i, joints_val[i]) for i in range(n_dofs)] -s_dot = idyntree.VectorDynSize(n_dofs) -[s_dot.setVal(i, joints_dot_val[i]) for i in range(n_dofs)] - -g = idyntree.Vector3() -g.zero() -g.setVal(2, -9.80665) -kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) -# set ADAM +g = np.array([0, 0, -9.80665]) H_b = utils.H_from_Pos_RPY(xyz, rpy) -vb_ = base_vel -s_ = joints_val -s_dot_ = joints_dot_val +kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) def test_mass_matrix(): @@ -104,8 +88,8 @@ def test_mass_matrix(): mass_mx = idyntree.MatrixDynSize() kinDyn.getFreeFloatingMassMatrix(mass_mx) mass_mxNumpy = mass_mx.toNumPy() - mass_test = cs.DM(M(H_b, s_)) - mass_test2 = cs.DM(comp.mass_matrix(H_b, s_)) + 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) @@ -115,17 +99,17 @@ def test_CMM(): cmm_idyntree = idyntree.MatrixDynSize() kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = cs.DM(Jcm(H_b, s_)) - Jcm_test2 = cs.DM(comp.centroidal_momentum_matrix(H_b, s_)) + 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, s_)) + CoM_test = cs.DM(com_f(H_b, joints_val)) CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - CoM_test2 = cs.DM(comp.CoM_position(H_b, s_)) + 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) @@ -141,8 +125,8 @@ def test_jacobian(): iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = cs.DM(J_tot(H_b, s_)) - J_test2 = cs.DM(comp.jacobian("l_sole", H_b, s_)) + 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) @@ -152,8 +136,8 @@ def test_jacobian_non_actuated(): iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = cs.DM(J_tot(H_b, s_)) - J_test2 = cs.DM(comp.jacobian("head", H_b, s_)) + 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) @@ -162,10 +146,12 @@ 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, s_, vb_, s_dot_) @ np.concatenate((vb_, s_dot_)) + 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, s_, vb_, s_dot_) - ) @ np.concatenate((vb_, s_dot_)) + 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) @@ -175,8 +161,8 @@ def test_fk(): 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, s_)) - H_test2 = cs.DM(comp.forward_kinematics("l_sole", H_b, s_)) + 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) @@ -188,8 +174,8 @@ def test_fk_non_actuated(): 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, s_)) - H_test2 = cs.DM(comp.forward_kinematics("head", H_b, s_)) + 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) @@ -203,8 +189,8 @@ def test_bias_force(): (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) ) h = comp.bias_force_fun() - h_test = cs.DM(h(H_b, s_, vb_, s_dot_)) - h_test2 = cs.DM(comp.bias_force(H_b, s_, vb_, s_dot_)) + 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) @@ -212,43 +198,44 @@ def test_bias_force(): def test_coriolis_term(): g0 = idyntree.Vector3() g0.zero() - kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g0) + 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, s_, vb_, s_dot_)) - C_test2 = cs.DM(comp.coriolis_term(H_b, s_, vb_, s_dot_)) + 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(): - vb0 = idyntree.Twist.Zero() - s_dot0 = idyntree.VectorDynSize(n_dofs) + base_vel_0 = np.zeros(6) + joints_dot_val_0 = np.zeros(n_dofs) kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) - kinDyn.setRobotState(H_b_idyn, s, vb0, s_dot0, g) + 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, s_)) - G_test2 = cs.DM(comp.gravity_term(H_b, s_)) + 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(): - kinDyn.setRobotState(H_from_Pos_RPY_idyn(np.zeros(3), np.zeros(3)), s, vb, s_dot, g) + 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(s_)) - J_test2 = cs.DM(comp.relative_jacobian("l_sole", s_)) + 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) diff --git a/tests/mixed/test_NumPy_mixed.py b/tests/mixed/test_NumPy_mixed.py index 32f0912e..d4710441 100644 --- a/tests/mixed/test_NumPy_mixed.py +++ b/tests/mixed/test_NumPy_mixed.py @@ -76,32 +76,16 @@ def H_from_Pos_RPY_idyn(xyz, rpy): joints_val = (np.random.rand(n_dofs) - 0.5) * 5 joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 -# set iDynTree kinDyn -H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) -vb = idyntree.Twist.Zero() -[vb.setVal(i, base_vel[i]) for i in range(6)] - -s = idyntree.VectorDynSize(n_dofs) -[s.setVal(i, joints_val[i]) for i in range(n_dofs)] -s_dot = idyntree.VectorDynSize(n_dofs) -[s_dot.setVal(i, joints_dot_val[i]) for i in range(n_dofs)] - -g = idyntree.Vector3() -g.zero() -g.setVal(2, -9.80665) -kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) -# set ADAM +g = np.array([0, 0, -9.80665]) H_b = utils.H_from_Pos_RPY(xyz, rpy) -vb_ = base_vel -s_ = joints_val -s_dot_ = joints_dot_val +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() - mass_test = comp.mass_matrix(H_b, s_) + mass_test = comp.mass_matrix(H_b, joints_val) assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) @@ -109,12 +93,12 @@ def test_CMM(): cmm_idyntree = idyntree.MatrixDynSize() kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) + Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, s_) + CoM_test = comp.CoM_position(H_b, joints_val) CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) @@ -129,7 +113,7 @@ def test_jacobian(): iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("l_sole", H_b, s_) + J_test = comp.jacobian("l_sole", H_b, joints_val) assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) @@ -137,15 +121,15 @@ def test_jacobian_non_actuated(): iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("head", H_b, s_) + J_test = comp.jacobian("head", H_b, joints_val) assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) def test_jacobian_dot(): - J_dot = comp.jacobian_dot("l_sole", H_b, s_, vb_, s_dot_) + J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) Jdotnu = kinDyn.getFrameBiasAcc("l_sole") Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot @ np.concatenate((vb_, s_dot_)) + J_dot_nu_test = J_dot @ np.concatenate((base_vel, joints_dot_val)) assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) @@ -153,7 +137,7 @@ def test_fk(): H_idyntree = kinDyn.getWorldTransform("l_sole") p_idy2np = H_idyntree.getPosition().toNumPy() R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("l_sole", H_b, s_) + H_test = 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) @@ -162,7 +146,7 @@ def test_fk_non_actuated(): H_idyntree = kinDyn.getWorldTransform("head") p_idy2np = H_idyntree.getPosition().toNumPy() R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("head", H_b, s_) + H_test = 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) @@ -173,20 +157,19 @@ def test_bias_force(): h_iDyn_np = np.concatenate( (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) ) - h_test = comp.bias_force(H_b, s_, vb_, s_dot_) + h_test = 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) def test_coriolis_term(): - g0 = idyntree.Vector3() - g0.zero() - kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g0) + g0 = np.zeros(3) + 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_test = comp.coriolis_term(H_b, s_, vb_, s_dot_) + C_test = 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) @@ -195,13 +178,13 @@ def test_gravity_term(): kinDyn2.loadRobotModel(robot_iDyn.model()) kinDyn2.setFloatingBase(root_link) kinDyn2.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) - vb0 = idyntree.Twist.Zero() - s_dot0 = idyntree.VectorDynSize(n_dofs) - kinDyn2.setRobotState(H_b_idyn, s, vb0, s_dot0, g) + base_vel0 = np.zeros(6) + joints_dot_val0 = np.zeros(n_dofs) + kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) assert kinDyn2.generalizedBiasForces(G_iDyn) G_iDyn_np = np.concatenate( (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) ) - G_test = comp.gravity_term(H_b, s_) + G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) From 158dec685bf86aa01b298c13ff6a51b116e73de5 Mon Sep 17 00:00:00 2001 From: giulero Date: Sun, 15 Oct 2023 11:44:03 +0200 Subject: [PATCH 37/39] Fix and update array handle --- src/adam/casadi/casadi_like.py | 2 +- src/adam/core/rbd_algorithms.py | 8 ++++++-- src/adam/jax/jax_like.py | 2 +- src/adam/numpy/numpy_like.py | 2 +- src/adam/pytorch/torch_like.py | 2 +- 5 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/adam/casadi/casadi_like.py b/src/adam/casadi/casadi_like.py index 522a74e0..ebd9f3fe 100644 --- a/src/adam/casadi/casadi_like.py +++ b/src/adam/casadi/casadi_like.py @@ -128,7 +128,7 @@ def array(*x) -> "CasadiLike": Returns: CasadiLike: Vector wrapping *x """ - return CasadiLike(cs.DM(*x)) + return CasadiLike(cs.SX(*x)) class SpatialMath(SpatialMath): diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 491c6f20..ba68391f 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -197,7 +197,9 @@ def jacobian( return J_tot # let's move to mixed representation elif self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: - w_H_L = base_transform @ B_H_L.array + if type(base_transform) != type(B_H_L): + base_transform = self.math.factory.array(base_transform) + w_H_L = base_transform @ B_H_L LI_X_L = self.math.adjoint_mixed(w_H_L) X = self.math.factory.eye(6 + self.NDoF) X[:6, :6] = self.math.adjoint_mixed_inverse(base_transform) @@ -302,7 +304,9 @@ def jacobian_dot( return J_dot # let's move to mixed representation elif self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: - I_H_L = base_transform @ B_H_j.array + if type(base_transform) != type(B_H_L): + base_transform = self.math.factory.array(base_transform) + I_H_L = base_transform @ B_H_j LI_X_L = self.math.adjoint_mixed(I_H_L) X = self.math.factory.eye(6 + self.NDoF) X[:6, :6] = self.math.adjoint_mixed_inverse(base_transform) diff --git a/src/adam/jax/jax_like.py b/src/adam/jax/jax_like.py index 1a2a7982..83df661f 100644 --- a/src/adam/jax/jax_like.py +++ b/src/adam/jax/jax_like.py @@ -126,7 +126,7 @@ def eye(x) -> "JaxLike": return JaxLike(jnp.eye(x)) @staticmethod - def array(*x) -> "JaxLike": + def array(x) -> "JaxLike": """ Returns: JaxLike: Vector wrapping *x diff --git a/src/adam/numpy/numpy_like.py b/src/adam/numpy/numpy_like.py index ed577159..85ee2f78 100644 --- a/src/adam/numpy/numpy_like.py +++ b/src/adam/numpy/numpy_like.py @@ -128,7 +128,7 @@ def eye(x: int) -> "NumpyLike": return NumpyLike(np.eye(x)) @staticmethod - def array(*x) -> "NumpyLike": + def array(x) -> "NumpyLike": """ Returns: NumpyLike: Vector wrapping *x diff --git a/src/adam/pytorch/torch_like.py b/src/adam/pytorch/torch_like.py index 50eaf541..a1afde00 100644 --- a/src/adam/pytorch/torch_like.py +++ b/src/adam/pytorch/torch_like.py @@ -134,7 +134,7 @@ def eye(x: int) -> "TorchLike": return TorchLike(torch.eye(x).float()) @staticmethod - def array(*x: ntp.ArrayLike) -> "TorchLike": + def array(x: ntp.ArrayLike) -> "TorchLike": """ Returns: TorchLike: vector wrapping x From 9d38f83a6abf1351873236ced23ce31f00124450 Mon Sep 17 00:00:00 2001 From: giulero Date: Sun, 15 Oct 2023 11:44:16 +0200 Subject: [PATCH 38/39] Update tests --- tests/body_fixed/test_CasADi_body_fixed.py | 88 ++++++++---------- tests/body_fixed/test_Jax_body_fixed.py | 76 ++++++---------- tests/body_fixed/test_NumPy_body_fixed.py | 69 +++++--------- tests/body_fixed/test_pytorch_body_fixed.py | 99 ++++++++------------- tests/mixed/test_Jax_mixed.py | 77 +++++++--------- tests/mixed/test_pytorch_mixed.py | 96 ++++++++------------ 6 files changed, 190 insertions(+), 315 deletions(-) diff --git a/tests/body_fixed/test_CasADi_body_fixed.py b/tests/body_fixed/test_CasADi_body_fixed.py index 52052b1f..d9686912 100644 --- a/tests/body_fixed/test_CasADi_body_fixed.py +++ b/tests/body_fixed/test_CasADi_body_fixed.py @@ -10,9 +10,9 @@ import numpy as np import pytest -import adam from adam.casadi import KinDynComputations from adam.geometry import utils +from adam import Representations np.random.seed(42) @@ -60,8 +60,7 @@ def H_from_Pos_RPY_idyn(xyz, rpy): root_link = "root_link" comp = KinDynComputations(model_path, joints_name_list, root_link) -comp.set_frame_velocity_representation(adam.Representations.BODY_FIXED_REPRESENTATION) - +comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) robot_iDyn = idyntree.ModelLoader() robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) @@ -79,25 +78,9 @@ def H_from_Pos_RPY_idyn(xyz, rpy): joints_val = (np.random.rand(n_dofs) - 0.5) * 5 joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 -# set iDynTree kinDyn -H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) -vb = idyntree.Twist.Zero() -[vb.setVal(i, base_vel[i]) for i in range(6)] - -s = idyntree.VectorDynSize(n_dofs) -[s.setVal(i, joints_val[i]) for i in range(n_dofs)] -s_dot = idyntree.VectorDynSize(n_dofs) -[s_dot.setVal(i, joints_dot_val[i]) for i in range(n_dofs)] - -g = idyntree.Vector3() -g.zero() -g.setVal(2, -9.80665) -kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) -# set ADAM +g = np.array([0, 0, -9.80665]) H_b = utils.H_from_Pos_RPY(xyz, rpy) -vb_ = base_vel -s_ = joints_val -s_dot_ = joints_dot_val +kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) def test_mass_matrix(): @@ -105,8 +88,8 @@ def test_mass_matrix(): mass_mx = idyntree.MatrixDynSize() kinDyn.getFreeFloatingMassMatrix(mass_mx) mass_mxNumpy = mass_mx.toNumPy() - mass_test = cs.DM(M(H_b, s_)) - mass_test2 = cs.DM(comp.mass_matrix(H_b, s_)) + 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) @@ -116,17 +99,17 @@ def test_CMM(): cmm_idyntree = idyntree.MatrixDynSize() kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = cs.DM(Jcm(H_b, s_)) - Jcm_test2 = cs.DM(comp.centroidal_momentum_matrix(H_b, s_)) + 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, s_)) + CoM_test = cs.DM(com_f(H_b, joints_val)) CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - CoM_test2 = cs.DM(comp.CoM_position(H_b, s_)) + 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) @@ -142,8 +125,8 @@ def test_jacobian(): iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = cs.DM(J_tot(H_b, s_)) - J_test2 = cs.DM(comp.jacobian("l_sole", H_b, s_)) + 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) @@ -153,8 +136,8 @@ def test_jacobian_non_actuated(): iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = cs.DM(J_tot(H_b, s_)) - J_test2 = cs.DM(comp.jacobian("head", H_b, s_)) + 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) @@ -163,10 +146,12 @@ 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, s_, vb_, s_dot_) @ np.concatenate((vb_, s_dot_)) + 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, s_, vb_, s_dot_) - ) @ np.concatenate((vb_, s_dot_)) + 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) @@ -176,8 +161,8 @@ def test_fk(): 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, s_)) - H_test2 = cs.DM(comp.forward_kinematics("l_sole", H_b, s_)) + 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) @@ -189,8 +174,8 @@ def test_fk_non_actuated(): 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, s_)) - H_test2 = cs.DM(comp.forward_kinematics("head", H_b, s_)) + 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) @@ -204,8 +189,8 @@ def test_bias_force(): (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) ) h = comp.bias_force_fun() - h_test = cs.DM(h(H_b, s_, vb_, s_dot_)) - h_test2 = cs.DM(comp.bias_force(H_b, s_, vb_, s_dot_)) + 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) @@ -213,43 +198,44 @@ def test_bias_force(): def test_coriolis_term(): g0 = idyntree.Vector3() g0.zero() - kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g0) + 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, s_, vb_, s_dot_)) - C_test2 = cs.DM(comp.coriolis_term(H_b, s_, vb_, s_dot_)) + 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(): - vb0 = idyntree.Twist.Zero() - s_dot0 = idyntree.VectorDynSize(n_dofs) + base_vel_0 = np.zeros(6) + joints_dot_val_0 = np.zeros(n_dofs) kinDyn.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) - kinDyn.setRobotState(H_b_idyn, s, vb0, s_dot0, g) + 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, s_)) - G_test2 = cs.DM(comp.gravity_term(H_b, s_)) + 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(): - kinDyn.setRobotState(H_from_Pos_RPY_idyn(np.zeros(3), np.zeros(3)), s, vb, s_dot, g) + 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(s_)) - J_test2 = cs.DM(comp.relative_jacobian("l_sole", s_)) + 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) diff --git a/tests/body_fixed/test_Jax_body_fixed.py b/tests/body_fixed/test_Jax_body_fixed.py index 5221f19b..10330947 100644 --- a/tests/body_fixed/test_Jax_body_fixed.py +++ b/tests/body_fixed/test_Jax_body_fixed.py @@ -63,7 +63,6 @@ def H_from_Pos_RPY_idyn(xyz, rpy): root_link = "root_link" comp = KinDynComputations(model_path, joints_name_list, root_link) comp.set_frame_velocity_representation(adam.Representations.BODY_FIXED_REPRESENTATION) - robot_iDyn = idyntree.ModelLoader() robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) @@ -81,45 +80,29 @@ def H_from_Pos_RPY_idyn(xyz, rpy): joints_val = (np.random.rand(n_dofs) - 0.5) * 5 joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 -# set iDynTree kinDyn -H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) -vb = idyntree.Twist.Zero() -[vb.setVal(i, base_vel[i]) for i in range(6)] - -s = idyntree.VectorDynSize(n_dofs) -s = s.FromPython(joints_val) -s_dot = idyntree.VectorDynSize(n_dofs) -s_dot = s_dot.FromPython(joints_dot_val) - -g = idyntree.Vector3() -g.zero() -g.setVal(2, -9.80665) -kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) -# set ADAM +g = np.array([0, 0, -9.80665]) H_b = utils.H_from_Pos_RPY(xyz, rpy) -vb_ = base_vel -s_ = joints_val -s_dot_ = joints_dot_val +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() -mass_test = comp.mass_matrix(H_b, s_) -assert np.asarray(mass_test) - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) +def test_mass_matrix(): + mass_mx = idyntree.MatrixDynSize() + kinDyn.getFreeFloatingMassMatrix(mass_mx) + mass_mxNumpy = mass_mx.toNumPy() + mass_test = comp.mass_matrix(H_b, joints_val) + assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) def test_CMM(): cmm_idyntree = idyntree.MatrixDynSize() kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) - assert np.array(Jcm_test) - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) + Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) + assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, s_) + CoM_test = comp.CoM_position(H_b, joints_val) CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) @@ -134,23 +117,23 @@ def test_jacobian(): iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("l_sole", H_b, s_) - assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-5) + J_test = comp.jacobian("l_sole", H_b, joints_val) + assert iDynNumpyJ_ - J_test == 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_test = comp.jacobian("head", H_b, s_) - assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-5) + J_test = comp.jacobian("head", H_b, joints_val) + assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) def test_jacobian_dot(): - J_dot = comp.jacobian_dot("l_sole", H_b, s_, vb_, s_dot_) + J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) Jdotnu = kinDyn.getFrameBiasAcc("l_sole") Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot @ np.concatenate((vb_, s_dot_)) + J_dot_nu_test = J_dot @ np.concatenate((base_vel, joints_dot_val)) assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) @@ -158,16 +141,16 @@ def test_fk(): H_idyntree = kinDyn.getWorldTransform("l_sole") p_idy2np = H_idyntree.getPosition().toNumPy() R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("l_sole", H_b, s_) - assert R_idy2np - np.array(H_test[:3, :3]) == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - np.array(H_test[:3, 3]) == pytest.approx(0.0, abs=1e-5) + H_test = 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) def test_fk_non_actuated(): H_idyntree = kinDyn.getWorldTransform("head") p_idy2np = H_idyntree.getPosition().toNumPy() R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("head", H_b, s_) + H_test = 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) @@ -178,20 +161,19 @@ def test_bias_force(): h_iDyn_np = np.concatenate( (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) ) - h_test = comp.bias_force(H_b, s_, vb_, s_dot_) + h_test = 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) def test_coriolis_term(): - g0 = idyntree.Vector3() - g0.zero() - kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g0) + g0 = np.zeros(3) + 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_test = comp.coriolis_term(H_b, s_, vb_, s_dot_) + C_test = 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) @@ -200,13 +182,13 @@ def test_gravity_term(): kinDyn2.loadRobotModel(robot_iDyn.model()) kinDyn2.setFloatingBase(root_link) kinDyn2.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) - vb0 = idyntree.Twist.Zero() - s_dot0 = idyntree.VectorDynSize(n_dofs) - kinDyn2.setRobotState(H_b_idyn, s, vb0, s_dot0, g) + base_vel0 = np.zeros(6) + joints_dot_val0 = np.zeros(n_dofs) + kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) assert kinDyn2.generalizedBiasForces(G_iDyn) G_iDyn_np = np.concatenate( (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) ) - G_test = comp.gravity_term(H_b, s_) + G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/body_fixed/test_NumPy_body_fixed.py b/tests/body_fixed/test_NumPy_body_fixed.py index 1e1ae465..5704c87d 100644 --- a/tests/body_fixed/test_NumPy_body_fixed.py +++ b/tests/body_fixed/test_NumPy_body_fixed.py @@ -9,7 +9,7 @@ import numpy as np import pytest -import adam +from adam import Representations from adam.geometry import utils from adam.numpy import KinDynComputations @@ -59,8 +59,7 @@ def H_from_Pos_RPY_idyn(xyz, rpy): root_link = "root_link" comp = KinDynComputations(model_path, joints_name_list, root_link) -comp.set_frame_velocity_representation(adam.Representations.BODY_FIXED_REPRESENTATION) - +comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) robot_iDyn = idyntree.ModelLoader() robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) @@ -78,32 +77,16 @@ def H_from_Pos_RPY_idyn(xyz, rpy): joints_val = (np.random.rand(n_dofs) - 0.5) * 5 joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 -# set iDynTree kinDyn -H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) -vb = idyntree.Twist.Zero() -[vb.setVal(i, base_vel[i]) for i in range(6)] - -s = idyntree.VectorDynSize(n_dofs) -[s.setVal(i, joints_val[i]) for i in range(n_dofs)] -s_dot = idyntree.VectorDynSize(n_dofs) -[s_dot.setVal(i, joints_dot_val[i]) for i in range(n_dofs)] - -g = idyntree.Vector3() -g.zero() -g.setVal(2, -9.80665) -kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) -# set ADAM +g = np.array([0, 0, -9.80665]) H_b = utils.H_from_Pos_RPY(xyz, rpy) -vb_ = base_vel -s_ = joints_val -s_dot_ = joints_dot_val +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() - mass_test = comp.mass_matrix(H_b, s_) + mass_test = comp.mass_matrix(H_b, joints_val) assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) @@ -111,12 +94,12 @@ def test_CMM(): cmm_idyntree = idyntree.MatrixDynSize() kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) + Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, s_) + CoM_test = comp.CoM_position(H_b, joints_val) CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) @@ -131,7 +114,7 @@ def test_jacobian(): iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("l_sole", H_b, s_) + J_test = comp.jacobian("l_sole", H_b, joints_val) assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) @@ -139,15 +122,15 @@ def test_jacobian_non_actuated(): iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("head", H_b, s_) + J_test = comp.jacobian("head", H_b, joints_val) assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) def test_jacobian_dot(): - J_dot = comp.jacobian_dot("l_sole", H_b, s_, vb_, s_dot_) + J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) Jdotnu = kinDyn.getFrameBiasAcc("l_sole") Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot @ np.concatenate((vb_, s_dot_)) + J_dot_nu_test = J_dot @ np.concatenate((base_vel, joints_dot_val)) assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) @@ -155,7 +138,7 @@ def test_fk(): H_idyntree = kinDyn.getWorldTransform("l_sole") p_idy2np = H_idyntree.getPosition().toNumPy() R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("l_sole", H_b, s_) + H_test = 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) @@ -164,7 +147,7 @@ def test_fk_non_actuated(): H_idyntree = kinDyn.getWorldTransform("head") p_idy2np = H_idyntree.getPosition().toNumPy() R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("head", H_b, s_) + H_test = 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) @@ -175,20 +158,19 @@ def test_bias_force(): h_iDyn_np = np.concatenate( (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) ) - h_test = comp.bias_force(H_b, s_, vb_, s_dot_) + h_test = 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) def test_coriolis_term(): - g0 = idyntree.Vector3() - g0.zero() - kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g0) + g0 = np.zeros(3) + 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_test = comp.coriolis_term(H_b, s_, vb_, s_dot_) + C_test = 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) @@ -197,22 +179,13 @@ def test_gravity_term(): kinDyn2.loadRobotModel(robot_iDyn.model()) kinDyn2.setFloatingBase(root_link) kinDyn2.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) - vb0 = idyntree.Twist.Zero() - s_dot0 = idyntree.VectorDynSize(n_dofs) - kinDyn2.setRobotState(H_b_idyn, s, vb0, s_dot0, g) + base_vel0 = np.zeros(6) + joints_dot_val0 = np.zeros(n_dofs) + kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) assert kinDyn2.generalizedBiasForces(G_iDyn) G_iDyn_np = np.concatenate( (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) ) - G_test = comp.gravity_term(H_b, s_) + G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) - - -def test_relative_jacobian(): - kinDyn.setRobotState(H_from_Pos_RPY_idyn(np.zeros(3), np.zeros(3)), s, vb, s_dot, g) - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] - J_test = comp.relative_jacobian("l_sole", s_) - assert iDynNumpyRelativeJ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) diff --git a/tests/body_fixed/test_pytorch_body_fixed.py b/tests/body_fixed/test_pytorch_body_fixed.py index a1bf3c4b..b0df67bb 100644 --- a/tests/body_fixed/test_pytorch_body_fixed.py +++ b/tests/body_fixed/test_pytorch_body_fixed.py @@ -10,7 +10,7 @@ import pytest import torch -import adam +from adam import Representations from adam.geometry import utils from adam.pytorch import KinDynComputations @@ -61,8 +61,7 @@ def H_from_Pos_RPY_idyn(xyz, rpy): root_link = "root_link" comp = KinDynComputations(model_path, joints_name_list, root_link) -comp.set_frame_velocity_representation(adam.Representations.BODY_FIXED_REPRESENTATION) - +comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) robot_iDyn = idyntree.ModelLoader() robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) @@ -80,52 +79,36 @@ def H_from_Pos_RPY_idyn(xyz, rpy): joints_val = (np.random.rand(n_dofs) - 0.5) * 5 joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 -# set iDynTree kinDyn -H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) -vb = idyntree.Twist.Zero() -[vb.setVal(i, base_vel[i]) for i in range(6)] - -s = idyntree.VectorDynSize(n_dofs) -[s.setVal(i, joints_val[i]) for i in range(n_dofs)] -s_dot = idyntree.VectorDynSize(n_dofs) -[s_dot.setVal(i, joints_dot_val[i]) for i in range(n_dofs)] - -g = idyntree.Vector3() -g.zero() -g.setVal(2, -9.80665) -kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) -# set ADAM -H_b = torch.FloatTensor(utils.H_from_Pos_RPY(xyz, rpy)) -vb_ = torch.FloatTensor(base_vel) -s_ = torch.FloatTensor(joints_val) -s_dot_ = torch.FloatTensor(joints_dot_val) +g = 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(): mass_mx = idyntree.MatrixDynSize() kinDyn.getFreeFloatingMassMatrix(mass_mx) mass_mxNumpy = mass_mx.toNumPy() - mass_test = comp.mass_matrix(H_b, s_) - assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-4) + mass_test = comp.mass_matrix(H_b, joints_val) + assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) def test_CMM(): cmm_idyntree = idyntree.MatrixDynSize() kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) - assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-4) + Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) + assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, s_) + CoM_test = comp.CoM_position(H_b, joints_val) CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-4) + assert CoM_test - 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-4 + 0.0, abs=1e-5 ) @@ -133,42 +116,42 @@ def test_jacobian(): iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("l_sole", H_b, s_) - assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) + J_test = comp.jacobian("l_sole", H_b, joints_val) + assert iDynNumpyJ_ - np.asarray(J_test) == 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_test = comp.jacobian("head", H_b, s_) - assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) + J_test = comp.jacobian("head", H_b, joints_val) + assert iDynNumpyJ_ - np.asarray(J_test) == pytest.approx(0.0, abs=1e-5) def test_jacobian_dot(): - J_dot = comp.jacobian_dot("l_sole", H_b, s_, vb_, s_dot_) + J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) Jdotnu = kinDyn.getFrameBiasAcc("l_sole") Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot @ np.concatenate((vb_, s_dot_)) - assert Jdot_nu - np.array(J_dot_nu_test) == pytest.approx(0.0, abs=1e-5) + J_dot_nu_test = J_dot @ np.concatenate((base_vel, joints_dot_val)) + assert Jdot_nu - np.asarray(J_dot_nu_test) == 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() - H_test = comp.forward_kinematics("l_sole", H_b, s_) - assert (R_idy2np) - np.array(H_test[:3, :3]) == pytest.approx(0.0, abs=1e-4) - assert (p_idy2np) - np.array(H_test[:3, 3]) == pytest.approx(0.0, abs=1e-4) + H_test = np.asarray(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) def test_fk_non_actuated(): H_idyntree = kinDyn.getWorldTransform("head") p_idy2np = H_idyntree.getPosition().toNumPy() R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("head", H_b, s_) - assert R_idy2np - np.array(H_test[:3, :3]) == pytest.approx(0.0, abs=1e-4) - assert p_idy2np - np.array(H_test[:3, 3]) == pytest.approx(0.0, abs=1e-4) + H_test = np.asarray(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) def test_bias_force(): @@ -177,21 +160,20 @@ def test_bias_force(): h_iDyn_np = np.concatenate( (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) ) - h_test = comp.bias_force(H_b, s_, vb_, s_dot_) - assert h_iDyn_np - np.array(h_test) == pytest.approx(0.0, abs=1e-4) + h_test = comp.bias_force(H_b, joints_val, base_vel, joints_dot_val) + assert h_iDyn_np - np.asarray(h_test) == pytest.approx(0.0, abs=1e-4) def test_coriolis_term(): - g0 = idyntree.Vector3() - g0.zero() - kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g0) + g0 = np.zeros(3) + 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_test = comp.coriolis_term(H_b, s_, vb_, s_dot_) - assert C_iDyn_np - np.array(C_test) == pytest.approx(0.0, abs=1e-4) + C_test = comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val) + assert C_iDyn_np - np.asarray(C_test) == pytest.approx(0.0, abs=1e-4) def test_gravity_term(): @@ -199,22 +181,13 @@ def test_gravity_term(): kinDyn2.loadRobotModel(robot_iDyn.model()) kinDyn2.setFloatingBase(root_link) kinDyn2.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) - vb0 = idyntree.Twist.Zero() - s_dot0 = idyntree.VectorDynSize(n_dofs) - kinDyn2.setRobotState(H_b_idyn, s, vb0, s_dot0, g) + base_vel0 = np.zeros(6) + joints_dot_val0 = np.zeros(n_dofs) + kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) assert kinDyn2.generalizedBiasForces(G_iDyn) G_iDyn_np = np.concatenate( (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) ) - G_test = comp.gravity_term(H_b, s_) - assert G_iDyn_np - np.array(G_test) == pytest.approx(0.0, abs=1e-4) - - -def test_relative_jacobian(): - kinDyn.setRobotState(H_from_Pos_RPY_idyn(np.zeros(3), np.zeros(3)), s, vb, s_dot, g) - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] - J_test = comp.relative_jacobian("l_sole", s_) - assert iDynNumpyRelativeJ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) + G_test = comp.gravity_term(H_b, joints_val) + assert G_iDyn_np - np.asarray(G_test) == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_Jax_mixed.py b/tests/mixed/test_Jax_mixed.py index 87343caa..636a3391 100644 --- a/tests/mixed/test_Jax_mixed.py +++ b/tests/mixed/test_Jax_mixed.py @@ -11,6 +11,7 @@ import pytest from jax import config +import adam from adam.geometry import utils from adam.jax import KinDynComputations @@ -61,6 +62,7 @@ def H_from_Pos_RPY_idyn(xyz, rpy): root_link = "root_link" comp = KinDynComputations(model_path, joints_name_list, root_link) +comp.set_frame_velocity_representation(adam.Representations.MIXED_REPRESENTATION) robot_iDyn = idyntree.ModelLoader() robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) @@ -78,45 +80,29 @@ def H_from_Pos_RPY_idyn(xyz, rpy): joints_val = (np.random.rand(n_dofs) - 0.5) * 5 joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 -# set iDynTree kinDyn -H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) -vb = idyntree.Twist.Zero() -[vb.setVal(i, base_vel[i]) for i in range(6)] - -s = idyntree.VectorDynSize(n_dofs) -s = s.FromPython(joints_val) -s_dot = idyntree.VectorDynSize(n_dofs) -s_dot = s_dot.FromPython(joints_dot_val) - -g = idyntree.Vector3() -g.zero() -g.setVal(2, -9.80665) -kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) -# set ADAM +g = np.array([0, 0, -9.80665]) H_b = utils.H_from_Pos_RPY(xyz, rpy) -vb_ = base_vel -s_ = joints_val -s_dot_ = joints_dot_val +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() -mass_test = comp.mass_matrix(H_b, s_) -assert np.asarray(mass_test) - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) +def test_mass_matrix(): + mass_mx = idyntree.MatrixDynSize() + kinDyn.getFreeFloatingMassMatrix(mass_mx) + mass_mxNumpy = mass_mx.toNumPy() + mass_test = comp.mass_matrix(H_b, joints_val) + assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) def test_CMM(): cmm_idyntree = idyntree.MatrixDynSize() kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) - assert np.array(Jcm_test) - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) + Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) + assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, s_) + CoM_test = comp.CoM_position(H_b, joints_val) CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) @@ -131,23 +117,23 @@ def test_jacobian(): iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("l_sole", H_b, s_) - assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-5) + J_test = comp.jacobian("l_sole", H_b, joints_val) + assert iDynNumpyJ_ - J_test == 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_test = comp.jacobian("head", H_b, s_) - assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-5) + J_test = comp.jacobian("head", H_b, joints_val) + assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) def test_jacobian_dot(): - J_dot = comp.jacobian_dot("l_sole", H_b, s_, vb_, s_dot_) + J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) Jdotnu = kinDyn.getFrameBiasAcc("l_sole") Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot @ np.concatenate((vb_, s_dot_)) + J_dot_nu_test = J_dot @ np.concatenate((base_vel, joints_dot_val)) assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) @@ -155,16 +141,16 @@ def test_fk(): H_idyntree = kinDyn.getWorldTransform("l_sole") p_idy2np = H_idyntree.getPosition().toNumPy() R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("l_sole", H_b, s_) - assert R_idy2np - np.array(H_test[:3, :3]) == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - np.array(H_test[:3, 3]) == pytest.approx(0.0, abs=1e-5) + H_test = 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) def test_fk_non_actuated(): H_idyntree = kinDyn.getWorldTransform("head") p_idy2np = H_idyntree.getPosition().toNumPy() R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("head", H_b, s_) + H_test = 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) @@ -175,20 +161,19 @@ def test_bias_force(): h_iDyn_np = np.concatenate( (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) ) - h_test = comp.bias_force(H_b, s_, vb_, s_dot_) + h_test = 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) def test_coriolis_term(): - g0 = idyntree.Vector3() - g0.zero() - kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g0) + g0 = np.zeros(3) + 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_test = comp.coriolis_term(H_b, s_, vb_, s_dot_) + C_test = 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) @@ -197,13 +182,13 @@ def test_gravity_term(): kinDyn2.loadRobotModel(robot_iDyn.model()) kinDyn2.setFloatingBase(root_link) kinDyn2.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) - vb0 = idyntree.Twist.Zero() - s_dot0 = idyntree.VectorDynSize(n_dofs) - kinDyn2.setRobotState(H_b_idyn, s, vb0, s_dot0, g) + base_vel0 = np.zeros(6) + joints_dot_val0 = np.zeros(n_dofs) + kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) assert kinDyn2.generalizedBiasForces(G_iDyn) G_iDyn_np = np.concatenate( (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) ) - G_test = comp.gravity_term(H_b, s_) + G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_pytorch_mixed.py b/tests/mixed/test_pytorch_mixed.py index bc07b098..de6a8229 100644 --- a/tests/mixed/test_pytorch_mixed.py +++ b/tests/mixed/test_pytorch_mixed.py @@ -10,6 +10,7 @@ import pytest import torch +from adam import Representations from adam.geometry import utils from adam.pytorch import KinDynComputations @@ -60,6 +61,7 @@ def H_from_Pos_RPY_idyn(xyz, rpy): root_link = "root_link" comp = KinDynComputations(model_path, joints_name_list, root_link) +comp.set_frame_velocity_representation(Representations.MIXED_REPRESENTATION) robot_iDyn = idyntree.ModelLoader() robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) @@ -77,52 +79,36 @@ def H_from_Pos_RPY_idyn(xyz, rpy): joints_val = (np.random.rand(n_dofs) - 0.5) * 5 joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 -# set iDynTree kinDyn -H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy) -vb = idyntree.Twist.Zero() -[vb.setVal(i, base_vel[i]) for i in range(6)] - -s = idyntree.VectorDynSize(n_dofs) -[s.setVal(i, joints_val[i]) for i in range(n_dofs)] -s_dot = idyntree.VectorDynSize(n_dofs) -[s_dot.setVal(i, joints_dot_val[i]) for i in range(n_dofs)] - -g = idyntree.Vector3() -g.zero() -g.setVal(2, -9.80665) -kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g) -# set ADAM -H_b = torch.FloatTensor(utils.H_from_Pos_RPY(xyz, rpy)) -vb_ = torch.FloatTensor(base_vel) -s_ = torch.FloatTensor(joints_val) -s_dot_ = torch.FloatTensor(joints_dot_val) +g = 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(): mass_mx = idyntree.MatrixDynSize() kinDyn.getFreeFloatingMassMatrix(mass_mx) mass_mxNumpy = mass_mx.toNumPy() - mass_test = comp.mass_matrix(H_b, s_) - assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-4) + mass_test = comp.mass_matrix(H_b, joints_val) + assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) def test_CMM(): cmm_idyntree = idyntree.MatrixDynSize() kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) - assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-4) + Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) + assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, s_) + CoM_test = comp.CoM_position(H_b, joints_val) CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-4) + assert CoM_test - 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-4 + 0.0, abs=1e-5 ) @@ -130,42 +116,42 @@ def test_jacobian(): iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("l_sole", H_b, s_) - assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) + J_test = comp.jacobian("l_sole", H_b, joints_val) + assert iDynNumpyJ_ - np.asarray(J_test) == 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_test = comp.jacobian("head", H_b, s_) - assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) + J_test = comp.jacobian("head", H_b, joints_val) + assert iDynNumpyJ_ - np.asarray(J_test) == pytest.approx(0.0, abs=1e-5) def test_jacobian_dot(): - J_dot = comp.jacobian_dot("l_sole", H_b, s_, vb_, s_dot_) + J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) Jdotnu = kinDyn.getFrameBiasAcc("l_sole") Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot @ np.concatenate((vb_, s_dot_)) - assert Jdot_nu - np.array(J_dot_nu_test) == pytest.approx(0.0, abs=1e-5) + J_dot_nu_test = J_dot @ np.concatenate((base_vel, joints_dot_val)) + assert Jdot_nu - np.asarray(J_dot_nu_test) == 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() - H_test = comp.forward_kinematics("l_sole", H_b, s_) - assert (R_idy2np) - np.array(H_test[:3, :3]) == pytest.approx(0.0, abs=1e-4) - assert (p_idy2np) - np.array(H_test[:3, 3]) == pytest.approx(0.0, abs=1e-4) + H_test = np.asarray(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) def test_fk_non_actuated(): H_idyntree = kinDyn.getWorldTransform("head") p_idy2np = H_idyntree.getPosition().toNumPy() R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("head", H_b, s_) - assert R_idy2np - np.array(H_test[:3, :3]) == pytest.approx(0.0, abs=1e-4) - assert p_idy2np - np.array(H_test[:3, 3]) == pytest.approx(0.0, abs=1e-4) + H_test = np.asarray(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) def test_bias_force(): @@ -174,21 +160,20 @@ def test_bias_force(): h_iDyn_np = np.concatenate( (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) ) - h_test = comp.bias_force(H_b, s_, vb_, s_dot_) - assert h_iDyn_np - np.array(h_test) == pytest.approx(0.0, abs=1e-4) + h_test = comp.bias_force(H_b, joints_val, base_vel, joints_dot_val) + assert h_iDyn_np - np.asarray(h_test) == pytest.approx(0.0, abs=1e-4) def test_coriolis_term(): - g0 = idyntree.Vector3() - g0.zero() - kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g0) + g0 = np.zeros(3) + 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_test = comp.coriolis_term(H_b, s_, vb_, s_dot_) - assert C_iDyn_np - np.array(C_test) == pytest.approx(0.0, abs=1e-4) + C_test = comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val) + assert C_iDyn_np - np.asarray(C_test) == pytest.approx(0.0, abs=1e-4) def test_gravity_term(): @@ -196,22 +181,13 @@ def test_gravity_term(): kinDyn2.loadRobotModel(robot_iDyn.model()) kinDyn2.setFloatingBase(root_link) kinDyn2.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) - vb0 = idyntree.Twist.Zero() - s_dot0 = idyntree.VectorDynSize(n_dofs) - kinDyn2.setRobotState(H_b_idyn, s, vb0, s_dot0, g) + base_vel0 = np.zeros(6) + joints_dot_val0 = np.zeros(n_dofs) + kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) assert kinDyn2.generalizedBiasForces(G_iDyn) G_iDyn_np = np.concatenate( (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) ) - G_test = comp.gravity_term(H_b, s_) - assert G_iDyn_np - np.array(G_test) == pytest.approx(0.0, abs=1e-4) - - -def test_relative_jacobian(): - kinDyn.setRobotState(H_from_Pos_RPY_idyn(np.zeros(3), np.zeros(3)), s, vb, s_dot, g) - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] - J_test = comp.relative_jacobian("l_sole", s_) - assert iDynNumpyRelativeJ - np.array(J_test) == pytest.approx(0.0, abs=1e-4) + G_test = comp.gravity_term(H_b, joints_val) + assert G_iDyn_np - np.asarray(G_test) == pytest.approx(0.0, abs=1e-4) From b1bc5e0810703537055a7118e7e9eb30ef223825 Mon Sep 17 00:00:00 2001 From: Giuseppe L'Erario Date: Sun, 15 Oct 2023 11:45:49 +0200 Subject: [PATCH 39/39] Update README.md Co-authored-by: Lorenzo Rapetti --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index f4399b13..72dc08f3 100644 --- a/README.md +++ b/README.md @@ -172,7 +172,7 @@ joints_name_list = [ # Specify the root link root_link = 'root_link' kinDyn = KinDynComputations(model_path, joints_name_list, root_link) -# choose the representation you want to use the body fixed representation +# choose the representation, if you want to use the body fixed representation kinDyn.set_frame_velocity_representation(adam.Representations.BODY_FIXED_REPRESENTATION) # or, if you want to use the mixed representation (that is the default) kinDyn.set_frame_velocity_representation(adam.Representations.MIXED_REPRESENTATION)