diff --git a/src/adam/parametric/pytorch/computations_parametric.py b/src/adam/parametric/pytorch/computations_parametric.py index 8e53eea1..531124f7 100644 --- a/src/adam/parametric/pytorch/computations_parametric.py +++ b/src/adam/parametric/pytorch/computations_parametric.py @@ -23,7 +23,9 @@ def __init__( joints_name_list: list, links_name_list: list, root_link: str = "root_link", - gravity: np.array = torch.FloatTensor([0, 0, -9.80665, 0, 0, 0]), + gravity: np.array = torch.tensor( + [0, 0, -9.80665, 0, 0, 0], dtype=torch.float64 + ), ) -> None: """ Args: @@ -119,7 +121,7 @@ def forward_kinematics( self, frame, base_transform: torch.Tensor, - s: torch.Tensor, + joint_positions: torch.Tensor, length_multiplier: torch.Tensor, densities: torch.Tensor, ) -> torch.Tensor: @@ -128,7 +130,7 @@ def forward_kinematics( 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 length_multiplier (torch.tensor): The length multiplier of the parametrized links densities (torch.tensor): The densities of the parametrized links @@ -149,7 +151,9 @@ def forward_kinematics( self.NDoF = self.rbdalgos.NDoF return ( self.rbdalgos.forward_kinematics( - frame, torch.FloatTensor(base_transform), torch.FloatTensor(s) + frame, + base_transform, + joint_positions, ) ).array @@ -414,7 +418,7 @@ def gravity_term( base_positions, torch.zeros(6).reshape(6, 1), torch.zeros(self.NDoF), - torch.FloatTensor(self.g), + self.g, ).array.squeeze() def get_total_mass( diff --git a/src/adam/pytorch/computations.py b/src/adam/pytorch/computations.py index f614acf9..e81c48be 100644 --- a/src/adam/pytorch/computations.py +++ b/src/adam/pytorch/computations.py @@ -21,7 +21,9 @@ def __init__( urdfstring: str, joints_name_list: list = None, root_link: str = "root_link", - gravity: np.array = torch.FloatTensor([0, 0, -9.80665, 0, 0, 0]), + gravity: np.array = torch.tensor( + [0, 0, -9.80665, 0, 0, 0], dtype=torch.float64 + ), ) -> None: """ Args: @@ -92,8 +94,8 @@ def forward_kinematics( return ( self.rbdalgos.forward_kinematics( frame, - torch.FloatTensor(base_transform), - torch.FloatTensor(joint_position), + base_transform, + joint_position, ) ).array @@ -240,7 +242,7 @@ def gravity_term( joint_positions, torch.zeros(6).reshape(6, 1), torch.zeros(self.NDoF), - torch.FloatTensor(self.g), + self.g, ).array.squeeze() def get_total_mass(self) -> float: diff --git a/src/adam/pytorch/torch_like.py b/src/adam/pytorch/torch_like.py index f24d91c5..71962f65 100644 --- a/src/adam/pytorch/torch_like.py +++ b/src/adam/pytorch/torch_like.py @@ -7,9 +7,9 @@ import numpy.typing as ntp import torch +import numpy as np from adam.core.spatial_math import ArrayLike, ArrayLikeFactory, SpatialMath -from adam.numpy import NumpyLike @dataclass @@ -18,12 +18,17 @@ class TorchLike(ArrayLike): array: torch.Tensor + def __post_init__(self): + """Converts array to double precision""" + if self.array.dtype != torch.float64: + self.array = self.array.double() + def __setitem__(self, idx, value: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": """Overrides set item operator""" if type(self) is type(value): self.array[idx] = value.array.reshape(self.array[idx].shape) else: - self.array[idx] = torch.tensor(value) + self.array[idx] = torch.tensor(value) if isinstance(value, float) else value def __getitem__(self, idx): """Overrides get item operator""" @@ -42,24 +47,29 @@ def T(self) -> "TorchLike": Returns: TorchLike: transpose of array """ - if len(self.array.shape) != 1: - return TorchLike(self.array.mT) + # check if self.array is a 0-D tensor + + if len(self.array.shape) == 0: + return TorchLike(self.array) x = self.array return TorchLike(x.permute(*torch.arange(x.ndim - 1, -1, -1))) def __matmul__(self, other: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": """Overrides @ operator""" + if type(self) is type(other): - return TorchLike(self.array @ other.array.float()) + return TorchLike(self.array @ other.array) + if isinstance(other, torch.Tensor): + return TorchLike(self.array @ other) else: - return TorchLike(self.array @ torch.tensor(other).float()) + return TorchLike(self.array @ torch.tensor(other)) def __rmatmul__(self, other: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": """Overrides @ operator""" if type(self) is type(other): return TorchLike(other.array @ self.array) else: - return TorchLike(torch.tensor(other).float() @ self.array) + return TorchLike(torch.tensor(other) @ self.array) def __mul__(self, other: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": """Overrides * operator""" @@ -120,7 +130,7 @@ def zeros(*x: int) -> "TorchLike": Returns: TorchLike: zero matrix of dimension *x """ - return TorchLike(torch.zeros(x).float()) + return TorchLike(torch.zeros(x)) @staticmethod def eye(x: int) -> "TorchLike": @@ -131,7 +141,7 @@ def eye(x: int) -> "TorchLike": Returns: TorchLike: identity matrix of dimension x """ - return TorchLike(torch.eye(x).float()) + return TorchLike(torch.eye(x)) @staticmethod def array(x: ntp.ArrayLike) -> "TorchLike": @@ -139,7 +149,7 @@ def array(x: ntp.ArrayLike) -> "TorchLike": Returns: TorchLike: vector wrapping x """ - return TorchLike(torch.FloatTensor(x)) + return TorchLike(torch.tensor(x)) class SpatialMath(SpatialMath): @@ -155,7 +165,8 @@ def sin(x: ntp.ArrayLike) -> "TorchLike": Returns: TorchLike: sin value of x """ - x = torch.tensor(x) + if isinstance(x, float): + x = torch.tensor(x) return TorchLike(torch.sin(x)) @staticmethod @@ -167,7 +178,9 @@ def cos(x: ntp.ArrayLike) -> "TorchLike": Returns: TorchLike: cos value of x """ - x = torch.tensor(x) + # transform to torch tensor, if not already + if isinstance(x, float): + x = torch.tensor(x) return TorchLike(torch.cos(x)) @staticmethod @@ -193,13 +206,11 @@ def skew(x: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": """ if not isinstance(x, TorchLike): return TorchLike( - torch.FloatTensor( - [[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]] - ) + torch.tensor([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]]) ) x = x.array return TorchLike( - torch.FloatTensor([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]]) + torch.tensor([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]]) ) @staticmethod @@ -211,7 +222,7 @@ def vertcat(*x: ntp.ArrayLike) -> "TorchLike": if isinstance(x[0], TorchLike): v = torch.vstack([x[i].array for i in range(len(x))]) else: - v = torch.FloatTensor(x) + v = torch.tensor(x) return TorchLike(v) @staticmethod @@ -223,5 +234,5 @@ def horzcat(*x: ntp.ArrayLike) -> "TorchLike": if isinstance(x[0], TorchLike): v = torch.hstack([x[i].array for i in range(len(x))]) else: - v = torch.FloatTensor(x) + v = torch.tensor(x) return TorchLike(v) diff --git a/tests/body_fixed/test_pytorch_body_fixed.py b/tests/body_fixed/test_pytorch_body_fixed.py index b0df67bb..daa3a142 100644 --- a/tests/body_fixed/test_pytorch_body_fixed.py +++ b/tests/body_fixed/test_pytorch_body_fixed.py @@ -13,6 +13,7 @@ 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) @@ -71,17 +72,18 @@ def H_from_Pos_RPY_idyn(xyz, rpy): 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 +xyz = (torch.rand(3) - 0.5) * 5 +rpy = (torch.rand(3) - 0.5) * 5 +base_vel = (torch.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 +joints_val = (torch.rand(n_dofs) - 0.5) * 5 +joints_dot_val = (torch.rand(n_dofs) - 0.5) * 5 -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) +g = torch.tensor([0, 0, -9.80665]) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array +kinDyn.setRobotState( + H_b.numpy(), joints_val.numpy(), base_vel.numpy(), joints_dot_val.numpy(), g.numpy() +) def test_mass_matrix(): @@ -165,8 +167,14 @@ def test_bias_force(): def test_coriolis_term(): - g0 = np.zeros(3) - kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) + g0 = torch.zeros(3) + kinDyn.setRobotState( + H_b.numpy(), + joints_val.numpy(), + base_vel.numpy(), + joints_dot_val.numpy(), + g0.numpy(), + ) C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) assert kinDyn.generalizedBiasForces(C_iDyn) C_iDyn_np = np.concatenate( @@ -181,9 +189,15 @@ def test_gravity_term(): kinDyn2.loadRobotModel(robot_iDyn.model()) kinDyn2.setFloatingBase(root_link) kinDyn2.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) - base_vel0 = np.zeros(6) - joints_dot_val0 = np.zeros(n_dofs) - kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) + base_vel0 = torch.zeros(6) + joints_dot_val0 = torch.zeros(n_dofs) + kinDyn2.setRobotState( + H_b.numpy(), + joints_val.numpy(), + base_vel0.numpy(), + joints_dot_val0.numpy(), + g.numpy(), + ) G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) assert kinDyn2.generalizedBiasForces(G_iDyn) G_iDyn_np = np.concatenate( diff --git a/tests/mixed/test_pytorch_mixed.py b/tests/mixed/test_pytorch_mixed.py index de6a8229..32a72702 100644 --- a/tests/mixed/test_pytorch_mixed.py +++ b/tests/mixed/test_pytorch_mixed.py @@ -13,6 +13,7 @@ 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) @@ -71,17 +72,18 @@ def H_from_Pos_RPY_idyn(xyz, rpy): 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 +xyz = (torch.rand(3) - 0.5) * 5 +rpy = (torch.rand(3) - 0.5) * 5 +base_vel = (torch.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 +joints_val = (torch.rand(n_dofs) - 0.5) * 5 +joints_dot_val = (torch.rand(n_dofs) - 0.5) * 5 -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) +g = torch.tensor([0, 0, -9.80665]) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array +kinDyn.setRobotState( + H_b.numpy(), joints_val.numpy(), base_vel.numpy(), joints_dot_val.numpy(), g.numpy() +) def test_mass_matrix(): @@ -165,8 +167,14 @@ def test_bias_force(): def test_coriolis_term(): - g0 = np.zeros(3) - kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) + g0 = torch.zeros(3) + kinDyn.setRobotState( + H_b.numpy(), + joints_val.numpy(), + base_vel.numpy(), + joints_dot_val.numpy(), + g0.numpy(), + ) C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) assert kinDyn.generalizedBiasForces(C_iDyn) C_iDyn_np = np.concatenate( @@ -181,9 +189,15 @@ def test_gravity_term(): kinDyn2.loadRobotModel(robot_iDyn.model()) kinDyn2.setFloatingBase(root_link) kinDyn2.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) - base_vel0 = np.zeros(6) - joints_dot_val0 = np.zeros(n_dofs) - kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) + base_vel0 = torch.zeros(6) + joints_dot_val0 = torch.zeros(n_dofs) + kinDyn2.setRobotState( + H_b.numpy(), + joints_val.numpy(), + base_vel0.numpy(), + joints_dot_val0.numpy(), + g.numpy(), + ) G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) assert kinDyn2.generalizedBiasForces(G_iDyn) G_iDyn_np = np.concatenate( diff --git a/tests/parametric/test_pytorch_computations_parametric.py b/tests/parametric/test_pytorch_computations_parametric.py index f40a89ee..ba51df96 100644 --- a/tests/parametric/test_pytorch_computations_parametric.py +++ b/tests/parametric/test_pytorch_computations_parametric.py @@ -11,6 +11,7 @@ import numpy as np from adam.parametric.pytorch import KinDynComputationsParametric from adam.pytorch import KinDynComputations +from adam.pytorch.torch_like import SpatialMath from adam.geometry import utils import tempfile @@ -61,11 +62,6 @@ "r_ankle_roll", ] - -def SX2DM(x): - return cs.DM(x) - - logging.basicConfig(level=logging.DEBUG) logging.debug("Showing the robot tree.") @@ -77,8 +73,6 @@ def SX2DM(x): comp_w_hardware = KinDynComputationsParametric( model_path, joints_name_list, link_name_list, root_link ) -# comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) -# comp_w_hardware.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) original_density = [ 628.0724496264945 @@ -87,41 +81,39 @@ def SX2DM(x): 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 +xyz = (torch.rand(3) - 0.5) * 5 +rpy = (torch.rand(3) - 0.5) * 5 +base_vel = (torch.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 +joints_val = (torch.rand(n_dofs) - 0.5) * 5 +joints_dot_val = (torch.rand(n_dofs) - 0.5) * 5 -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 = torch.tensor([0, 0, -9.80665]) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array def test_mass_matrix(): - mass_test = comp.mass_matrix(H_b, s_) + mass_test = comp.mass_matrix(H_b, joints_val) mass_test_hardware = np.array( - comp_w_hardware.mass_matrix(H_b, s_, original_length, original_density) + comp_w_hardware.mass_matrix(H_b, joints_val, original_length, original_density) ) assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5) def test_CMM(): - Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) + Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) Jcm_test_hardware = np.array( comp_w_hardware.centroidal_momentum_matrix( - H_b, s_, original_length, original_density + H_b, joints_val, original_length, original_density ) ) assert Jcm_test - Jcm_test_hardware == 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_hardware = np.array( - comp_w_hardware.CoM_position(H_b, s_, original_length, original_density) + comp_w_hardware.CoM_position(H_b, joints_val, original_length, original_density) ) assert CoM_test - CoM_hardware == pytest.approx(0.0, abs=1e-5) @@ -133,17 +125,21 @@ def test_total_mass(): def test_jacobian(): - J_test = comp.jacobian("l_sole", H_b, s_) + J_test = comp.jacobian("l_sole", H_b, joints_val) J_test_hardware = np.array( - comp_w_hardware.jacobian("l_sole", H_b, s_, original_length, original_density) + comp_w_hardware.jacobian( + "l_sole", H_b, joints_val, original_length, original_density + ) ) assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) def test_jacobian_non_actuated(): - J_test = comp.jacobian("head", H_b, s_) + J_test = comp.jacobian("head", H_b, joints_val) J_test_hardware = np.array( - comp_w_hardware.jacobian("head", H_b, s_, original_length, original_density) + comp_w_hardware.jacobian( + "head", H_b, joints_val, original_length, original_density + ) ) assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) @@ -163,10 +159,10 @@ def test_jacobian_dot(): def test_fk(): - H_test = np.array(comp.forward_kinematics("l_sole", H_b, s_)) + H_test = np.array(comp.forward_kinematics("l_sole", H_b, joints_val)) H_with_hardware_test = np.array( comp_w_hardware.forward_kinematics( - "l_sole", H_b, s_, original_length, original_density + "l_sole", H_b, joints_val, original_length, original_density ) ) assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) @@ -174,10 +170,10 @@ def test_fk(): def test_fk_non_actuated(): - H_test = np.array(comp.forward_kinematics("head", H_b, s_)) + H_test = np.array(comp.forward_kinematics("head", H_b, joints_val)) H_with_hardware_test = np.array( comp_w_hardware.forward_kinematics( - "head", H_b, s_, original_length, original_density + "head", H_b, joints_val, original_length, original_density ) ) assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) @@ -185,28 +181,28 @@ def test_fk_non_actuated(): def test_bias_force(): - h_test = np.array(comp.bias_force(H_b, s_, vb_, s_dot_)) + h_test = np.array(comp.bias_force(H_b, joints_val, base_vel, joints_dot_val)) h_with_hardware_test = np.array( comp_w_hardware.bias_force( - H_b, s_, vb_, s_dot_, original_length, original_density + H_b, joints_val, base_vel, joints_dot_val, original_length, original_density ) ) assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4) def test_coriolis_term(): - C_test = np.array(comp.coriolis_term(H_b, s_, vb_, s_dot_)) + C_test = np.array(comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val)) C_with_hardware_test = np.array( comp_w_hardware.coriolis_term( - H_b, s_, vb_, s_dot_, original_length, original_density + H_b, joints_val, base_vel, joints_dot_val, original_length, original_density ) ) assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4) def test_gravity_term(): - G_test = comp.gravity_term(H_b, s_) + G_test = comp.gravity_term(H_b, joints_val) G_with_hardware_test = comp_w_hardware.gravity_term( - H_b, s_, original_length, original_density + H_b, joints_val, original_length, original_density ) assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4)