diff --git a/src/adam/geometry/__init__.py b/src/adam/geometry/__init__.py deleted file mode 100644 index 55006fb5..00000000 --- a/src/adam/geometry/__init__.py +++ /dev/null @@ -1,5 +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. - -from . import utils diff --git a/src/adam/geometry/utils.py b/src/adam/geometry/utils.py deleted file mode 100644 index f120546d..00000000 --- a/src/adam/geometry/utils.py +++ /dev/null @@ -1,115 +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 - - -def R_from_axis_angle(axis, q): - [cq, sq] = [np.cos(q), np.sin(q)] - return ( - cq * (np.eye(3) - np.outer(axis, axis)) - + sq * cs.skew(axis) - + np.outer(axis, axis) - ) - - -def Rx(q): - R = np.eye(3) - [cq, sq] = [np.cos(q), np.sin(q)] - R[1, 1] = cq - R[1, 2] = -sq - R[2, 1] = sq - R[2, 2] = cq - return R - - -def Ry(q): - R = np.eye(3) - [cq, sq] = [np.cos(q), np.sin(q)] - R[0, 0] = cq - R[0, 2] = sq - R[2, 0] = -sq - R[2, 2] = cq - return R - - -def Rz(q): - R = np.eye(3) - [cq, sq] = [np.cos(q), np.sin(q)] - R[0, 0] = cq - R[0, 1] = -sq - R[1, 0] = sq - R[1, 1] = cq - return R - - -def H_revolute_joint(xyz, rpy, axis, q): - T = np.eye(4) - R = R_from_RPY(rpy) @ R_from_axis_angle(axis, q) - T[:3, :3] = R - T[:3, 3] = xyz - return T - - -def H_from_Pos_RPY(xyz, rpy): - T = np.eye(4) - T[:3, :3] = R_from_RPY(rpy) - T[:3, 3] = xyz - return T - - -def R_from_RPY(rpy): - return Rz(rpy[2]) @ Ry(rpy[1]) @ Rx(rpy[0]) - - -def X_revolute_joint(xyz, rpy, axis, q): - T = H_revolute_joint(xyz, rpy, axis, q) - R = T[:3, :3].T - p = -T[:3, :3].T @ T[:3, 3] - return spatial_transform(R, p) - - -def X_fixed_joint(xyz, rpy): - T = H_from_Pos_RPY(xyz, rpy) - R = T[:3, :3].T - p = -T[:3, :3].T @ T[:3, 3] - return spatial_transform(R, p) - - -def spatial_transform(R, p): - X = cs.SX.zeros(6, 6) - X[:3, :3] = R - X[3:, 3:] = R - X[:3, 3:] = cs.skew(p) @ R - return X - - -def spatial_inertia(I, mass, c, rpy): - # Returns the 6x6 inertia matrix expressed at the origin of the link (with rotation)""" - IO = np.zeros([6, 6]) - Sc = cs.skew(c) - R = R_from_RPY(rpy) - inertia_matrix = np.array( - [[I.ixx, I.ixy, I.ixz], [I.ixy, I.iyy, I.iyz], [I.ixz, I.iyz, I.izz]] - ) - - IO[3:, 3:] = R @ inertia_matrix @ R.T + mass * Sc @ Sc.T - IO[3:, :3] = mass * Sc - IO[:3, 3:] = mass * Sc.T - IO[:3, :3] = np.eye(3) * mass - return IO - - -def spatial_skew(v): - X = cs.SX.zeros(6, 6) - X[:3, :3] = cs.skew(v[3:]) - X[:3, 3:] = cs.skew(v[:3]) - X[3:, 3:] = cs.skew(v[3:]) - return X - - -def spatial_skew_star(v): - return -spatial_skew(v).T diff --git a/tests/body_fixed/test_CasADi_body_fixed.py b/tests/body_fixed/test_CasADi_body_fixed.py index d9686912..8d5b8c0c 100644 --- a/tests/body_fixed/test_CasADi_body_fixed.py +++ b/tests/body_fixed/test_CasADi_body_fixed.py @@ -11,9 +11,10 @@ import pytest from adam.casadi import KinDynComputations -from adam.geometry import utils +from adam.numpy.numpy_like import SpatialMath from adam import Representations + np.random.seed(42) model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) @@ -79,7 +80,7 @@ def H_from_Pos_RPY_idyn(xyz, rpy): joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 g = np.array([0, 0, -9.80665]) -H_b = utils.H_from_Pos_RPY(xyz, rpy) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) diff --git a/tests/body_fixed/test_Jax_body_fixed.py b/tests/body_fixed/test_Jax_body_fixed.py index 10330947..1bfc8eb3 100644 --- a/tests/body_fixed/test_Jax_body_fixed.py +++ b/tests/body_fixed/test_Jax_body_fixed.py @@ -12,8 +12,9 @@ from jax import config import adam -from adam.geometry import utils from adam.jax import KinDynComputations +from adam.numpy.numpy_like import SpatialMath + np.random.seed(42) config.update("jax_enable_x64", True) @@ -81,7 +82,7 @@ def H_from_Pos_RPY_idyn(xyz, rpy): joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 g = np.array([0, 0, -9.80665]) -H_b = utils.H_from_Pos_RPY(xyz, rpy) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) diff --git a/tests/body_fixed/test_NumPy_body_fixed.py b/tests/body_fixed/test_NumPy_body_fixed.py index 5704c87d..51b297f4 100644 --- a/tests/body_fixed/test_NumPy_body_fixed.py +++ b/tests/body_fixed/test_NumPy_body_fixed.py @@ -10,8 +10,9 @@ import pytest from adam import Representations -from adam.geometry import utils from adam.numpy import KinDynComputations +from adam.numpy.numpy_like import SpatialMath + np.random.seed(42) @@ -78,7 +79,7 @@ def H_from_Pos_RPY_idyn(xyz, rpy): joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 g = np.array([0, 0, -9.80665]) -H_b = utils.H_from_Pos_RPY(xyz, rpy) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) diff --git a/tests/body_fixed/test_pytorch_body_fixed.py b/tests/body_fixed/test_pytorch_body_fixed.py index daa3a142..a6a1bba8 100644 --- a/tests/body_fixed/test_pytorch_body_fixed.py +++ b/tests/body_fixed/test_pytorch_body_fixed.py @@ -11,10 +11,10 @@ import torch from adam import Representations -from adam.geometry import utils from adam.pytorch import KinDynComputations from adam.pytorch.torch_like import SpatialMath + np.random.seed(42) torch.set_default_dtype(torch.float64) diff --git a/tests/conversions/test_idyntree.py b/tests/conversions/test_idyntree.py index 6f26a1cc..975e33be 100644 --- a/tests/conversions/test_idyntree.py +++ b/tests/conversions/test_idyntree.py @@ -15,10 +15,11 @@ import pytest from adam.casadi import KinDynComputations -from adam.geometry import utils +from adam.numpy.numpy_like import SpatialMath from adam import Representations from adam.model.conversions.idyntree import to_idyntree_model + np.random.seed(42) model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) @@ -82,7 +83,7 @@ def H_from_Pos_RPY_idyn(xyz, rpy): joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 g = np.array([0, 0, -9.80665]) -H_b = utils.H_from_Pos_RPY(xyz, rpy) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) diff --git a/tests/conversions/test_idyntree_parametric.py b/tests/conversions/test_idyntree_parametric.py index 734cee5f..6b6862b3 100644 --- a/tests/conversions/test_idyntree_parametric.py +++ b/tests/conversions/test_idyntree_parametric.py @@ -15,11 +15,12 @@ ) from adam.model.conversions.idyntree import to_idyntree_model from adam.core.constants import Representations +from adam.numpy.numpy_like import SpatialMath -from adam.geometry import utils import tempfile from git import Repo + # Getting stickbot urdf file temp_dir = tempfile.TemporaryDirectory() git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git" @@ -96,7 +97,7 @@ joints_val = (np.random.rand(n_dofs) - 0.5) * 5 joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 -H_b = utils.H_from_Pos_RPY(xyz, rpy) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array vb_ = base_vel s_ = joints_val s_dot_ = joints_dot_val diff --git a/tests/mixed/test_CasADi_mixed.py b/tests/mixed/test_CasADi_mixed.py index 60c1c102..bc0134f8 100644 --- a/tests/mixed/test_CasADi_mixed.py +++ b/tests/mixed/test_CasADi_mixed.py @@ -11,9 +11,10 @@ import pytest from adam.casadi import KinDynComputations -from adam.geometry import utils +from adam.numpy.numpy_like import SpatialMath from adam import Representations + np.random.seed(42) model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) @@ -79,7 +80,7 @@ def H_from_Pos_RPY_idyn(xyz, rpy): joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 g = np.array([0, 0, -9.80665]) -H_b = utils.H_from_Pos_RPY(xyz, rpy) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) diff --git a/tests/mixed/test_Jax_mixed.py b/tests/mixed/test_Jax_mixed.py index 636a3391..af4bd7bc 100644 --- a/tests/mixed/test_Jax_mixed.py +++ b/tests/mixed/test_Jax_mixed.py @@ -12,8 +12,9 @@ from jax import config import adam -from adam.geometry import utils from adam.jax import KinDynComputations +from adam.numpy.numpy_like import SpatialMath + np.random.seed(42) config.update("jax_enable_x64", True) @@ -81,7 +82,7 @@ def H_from_Pos_RPY_idyn(xyz, rpy): joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 g = np.array([0, 0, -9.80665]) -H_b = utils.H_from_Pos_RPY(xyz, rpy) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) diff --git a/tests/mixed/test_NumPy_mixed.py b/tests/mixed/test_NumPy_mixed.py index d4710441..1a651bed 100644 --- a/tests/mixed/test_NumPy_mixed.py +++ b/tests/mixed/test_NumPy_mixed.py @@ -10,8 +10,9 @@ import pytest from adam import Representations -from adam.geometry import utils from adam.numpy import KinDynComputations +from adam.numpy.numpy_like import SpatialMath + np.random.seed(42) @@ -77,7 +78,7 @@ def H_from_Pos_RPY_idyn(xyz, rpy): joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 g = np.array([0, 0, -9.80665]) -H_b = utils.H_from_Pos_RPY(xyz, rpy) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) diff --git a/tests/mixed/test_pytorch_mixed.py b/tests/mixed/test_pytorch_mixed.py index 32a72702..3ce33b27 100644 --- a/tests/mixed/test_pytorch_mixed.py +++ b/tests/mixed/test_pytorch_mixed.py @@ -11,10 +11,10 @@ import torch from adam import Representations -from adam.geometry import utils from adam.pytorch import KinDynComputations from adam.pytorch.torch_like import SpatialMath + np.random.seed(42) torch.set_default_dtype(torch.float64) diff --git a/tests/parametric/test_CasADi_computations_parametric.py b/tests/parametric/test_CasADi_computations_parametric.py index b36d9961..51dc7888 100644 --- a/tests/parametric/test_CasADi_computations_parametric.py +++ b/tests/parametric/test_CasADi_computations_parametric.py @@ -10,12 +10,13 @@ import math from adam.parametric.casadi import KinDynComputationsParametric from adam.casadi import KinDynComputations +from adam.numpy.numpy_like import SpatialMath -from adam.geometry import utils import tempfile from git import Repo from adam import Representations + # Getting stickbot urdf file temp_dir = tempfile.TemporaryDirectory() git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git" @@ -88,7 +89,7 @@ joints_val = (np.random.rand(n_dofs) - 0.5) * 5 joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 -H_b = utils.H_from_Pos_RPY(xyz, rpy) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array vb_ = base_vel s_ = joints_val s_dot_ = joints_dot_val diff --git a/tests/parametric/test_Jax_computations_parametric.py b/tests/parametric/test_Jax_computations_parametric.py index aec6ee87..0aed8b54 100644 --- a/tests/parametric/test_Jax_computations_parametric.py +++ b/tests/parametric/test_Jax_computations_parametric.py @@ -12,12 +12,13 @@ import math from adam.parametric.jax import KinDynComputationsParametric from adam.jax import KinDynComputations +from adam.numpy.numpy_like import SpatialMath -from adam.geometry import utils import tempfile from git import Repo from adam import Representations + np.random.seed(42) config.update("jax_enable_x64", True) @@ -65,11 +66,6 @@ "r_ankle_roll", ] - -def SX2DM(x): - return cs.DM(x) - - logging.basicConfig(level=logging.DEBUG) logging.debug("Showing the robot tree.") @@ -96,7 +92,7 @@ def SX2DM(x): joints_val = (np.random.rand(n_dofs) - 0.5) * 5 joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 -H_b = utils.H_from_Pos_RPY(xyz, rpy) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array vb_ = base_vel s_ = joints_val s_dot_ = joints_dot_val diff --git a/tests/parametric/test_NumPy_computations_parametric.py b/tests/parametric/test_NumPy_computations_parametric.py index 42b51c9d..1354aa80 100644 --- a/tests/parametric/test_NumPy_computations_parametric.py +++ b/tests/parametric/test_NumPy_computations_parametric.py @@ -10,12 +10,13 @@ import numpy as np from adam.parametric.numpy import KinDynComputationsParametric from adam.numpy import KinDynComputations +from adam.numpy.numpy_like import SpatialMath -from adam.geometry import utils import tempfile from git import Repo from adam import Representations + np.random.seed(42) # Getting stickbot urdf file @@ -62,11 +63,6 @@ "r_ankle_roll", ] - -def SX2DM(x): - return cs.DM(x) - - logging.basicConfig(level=logging.DEBUG) logging.debug("Showing the robot tree.") @@ -95,7 +91,7 @@ def SX2DM(x): joints_val = (np.random.rand(n_dofs) - 0.5) * 5 joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 -H_b = utils.H_from_Pos_RPY(xyz, rpy) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array vb_ = base_vel s_ = joints_val s_dot_ = joints_dot_val diff --git a/tests/parametric/test_pytorch_computations_parametric.py b/tests/parametric/test_pytorch_computations_parametric.py index f2a18963..73fad13d 100644 --- a/tests/parametric/test_pytorch_computations_parametric.py +++ b/tests/parametric/test_pytorch_computations_parametric.py @@ -13,11 +13,11 @@ from adam.pytorch import KinDynComputations from adam.pytorch.torch_like import SpatialMath -from adam.geometry import utils import tempfile from git import Repo from adam import Representations + np.random.seed(42) torch.set_default_dtype(torch.float64) diff --git a/tests/pytorch_batch/test_pytorch_batch.py b/tests/pytorch_batch/test_pytorch_batch.py index 44b3a33d..76251813 100644 --- a/tests/pytorch_batch/test_pytorch_batch.py +++ b/tests/pytorch_batch/test_pytorch_batch.py @@ -8,11 +8,13 @@ from jax import config import adam -from adam.geometry import utils from adam.pytorch import KinDynComputationsBatch from adam.numpy import KinDynComputations +from adam.numpy.numpy_like import SpatialMath + import torch + np.random.seed(42) config.update("jax_enable_x64", True) @@ -61,7 +63,7 @@ joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 g = np.array([0, 0, -9.80665]) -H_b = utils.H_from_Pos_RPY(xyz, rpy) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array n_samples = 10 H_b_batch = torch.tile(torch.tensor(H_b), (n_samples, 1, 1)).requires_grad_()