diff --git a/README.md b/README.md index 422445e8..72dc08f3 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, 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) 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) 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/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/casadi/computations.py b/src/adam/casadi/computations.py index 6d8ed568..1fb50e48 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -4,9 +4,11 @@ import casadi as cs import numpy as np +from typing import Union 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,16 +39,28 @@ 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 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 @@ -54,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 @@ -66,12 +82,14 @@ 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 """ - 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) + H = self.rbdalgos.forward_kinematics(frame, base_transform, joint_positions) + return cs.Function( + "H", [base_transform, joint_positions], [H.array], self.f_opts + ) def jacobian_fun(self, frame: str) -> cs.Function: """Returns the Jacobian relative to the specified frame @@ -82,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 @@ -96,20 +116,45 @@ 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 + + Args: + frame (str): The frame to which the jacobian will be computed + + Returns: + J_dot (casADi function): The Jacobian derivative relative to the frame + """ + 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 Returns: - com (casADi function): The CoM position + 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, @@ -118,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, @@ -132,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, @@ -147,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 + H (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 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 diff --git a/src/adam/core/constants.py b/src/adam/core/constants.py new file mode 100644 index 00000000..44ef2d35 --- /dev/null +++ b/src/adam/core/constants.py @@ -0,0 +1,6 @@ +from enum import IntEnum, auto + + +class Representations(IntEnum): + BODY_FIXED_REPRESENTATION = auto() + MIXED_REPRESENTATION = auto() diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 520533f4..ba68391f 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 @@ -103,14 +115,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 @@ -122,19 +140,19 @@ 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_L (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_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 - T_joint = joint.homogeneous(q=q_) - T_fk = T_fk @ T_joint - return T_fk + H_joint = joint.homogeneous(q=q_) + I_H_L = I_H_L @ H_joint + return I_H_L 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 @@ -147,57 +165,166 @@ 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_j = 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_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 - 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_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.vertcat(J_lin, J_ang) - + J[:, joint.idx] = self.math.adjoint(L_H_j) @ joint.motion_subspace() return J 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, joint_positions) + 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[: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: + 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) + 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 ) -> 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 """ - base_transform = self.math.factory.eye(4) - return self.joints_jacobian(frame, base_transform, 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, + 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) + # 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) + # 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_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 + J_dot[:, joint.idx + 6] = J_dot_j + + if ( + self.frame_velocity_representation + == Representations.BODY_FIXED_REPRESENTATION + ): + return J_dot + # let's move to mixed representation + elif self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: + 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) + 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) + 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: + raise NotImplementedError( + "Only BODY_FIXED_REPRESENTATION and MIXED_REPRESENTATION are implemented" + ) def CoM_position( self, base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike @@ -214,11 +341,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 @@ -263,20 +390,33 @@ def rnea( a = [None] * model_len f = [None] * model_len - X_to_mixed = self.math.adjoint(base_transform[:3, :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" + ) - 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:] - ) # 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() @@ -287,7 +427,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 @@ -314,7 +454,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): diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py index 296ab0dd..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 @@ -404,14 +405,145 @@ 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_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: + 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_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: + 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 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 = self.skew(v[3:]) @ R + 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 = 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 + 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 diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index eaf64407..766e8a81 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -4,8 +4,8 @@ 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 adam.jax.jax_like import SpatialMath from adam.model import Model, URDFModelFactory @@ -36,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: jnp.array, joint_positions: jnp.array): """Returns the Mass Matrix functions computed the CRBA @@ -76,6 +86,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 ): @@ -87,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 @@ -116,22 +150,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( @@ -139,16 +173,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 @@ -157,14 +191,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: @@ -192,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 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/computations.py b/src/adam/numpy/computations.py index e0fe77d6..316c0218 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: @@ -75,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 @@ -110,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: 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/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() diff --git a/src/adam/pytorch/computations.py b/src/adam/pytorch/computations.py index d2f257a9..c02d256f 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,57 +36,72 @@ 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 + 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 @@ -111,6 +127,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: @@ -121,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 @@ -130,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: @@ -139,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 @@ -148,7 +188,7 @@ def bias_force( """ return self.rbdalgos.rnea( base_transform, - s, + joint_positions, base_velocity.reshape(6, 1), joint_velocities, self.g, @@ -183,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), 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 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..d9686912 --- /dev/null +++ b/tests/body_fixed/test_CasADi_body_fixed.py @@ -0,0 +1,241 @@ +# 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 +from adam import Representations + +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(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 + +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(): + 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, 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) + + +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, 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, joints_val)) + CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() + 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) + + +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, 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) + + +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, 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) + + +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, 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, 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) + + +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, 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) + assert p_idy2np - H_test2[: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, 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) + assert p_idy2np - H_test2[: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, 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) + + +def test_coriolis_term(): + g0 = idyntree.Vector3() + g0.zero() + 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, 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(): + base_vel_0 = np.zeros(6) + joints_dot_val_0 = np.zeros(n_dofs) + kinDyn.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) + 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, 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(): + 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(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/test_Jax_computations.py b/tests/body_fixed/test_Jax_body_fixed.py similarity index 68% rename from tests/test_Jax_computations.py rename to tests/body_fixed/test_Jax_body_fixed.py index c5e43260..10330947 100644 --- a/tests/test_Jax_computations.py +++ b/tests/body_fixed/test_Jax_body_fixed.py @@ -11,6 +11,7 @@ import pytest from jax import config +import adam from adam.geometry import utils from adam.jax import KinDynComputations @@ -61,13 +62,14 @@ 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.BODY_FIXED_REPRESENTATION) n_dofs = len(joints_name_list) # base pose quantities @@ -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,32 +117,40 @@ 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, joints_val, base_vel, joints_dot_val) + Jdotnu = kinDyn.getFrameBiasAcc("l_sole") + Jdot_nu = Jdotnu.toNumPy() + 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) 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) @@ -167,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) @@ -188,14 +181,14 @@ def test_gravity_term(): kinDyn2 = idyntree.KinDynComputations() 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) + 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) 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 new file mode 100644 index 00000000..5704c87d --- /dev/null +++ b/tests/body_fixed/test_NumPy_body_fixed.py @@ -0,0 +1,191 @@ +# 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.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 + +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, 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, 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, joints_val) + 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, 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, 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, joints_val, base_vel, joints_dot_val) + Jdotnu = kinDyn.getFrameBiasAcc("l_sole") + Jdot_nu = Jdotnu.toNumPy() + 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) + + +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, 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, 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(): + 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, 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 = 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, joints_val, base_vel, joints_dot_val) + 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) + 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, joints_val) + assert G_iDyn_np - G_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..b0df67bb --- /dev/null +++ b/tests/body_fixed/test_pytorch_body_fixed.py @@ -0,0 +1,193 @@ +# 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 + +from adam import Representations +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(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 + +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, 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, 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, joints_val) + 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, 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, 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, joints_val, base_vel, joints_dot_val) + Jdotnu = kinDyn.getFrameBiasAcc("l_sole") + Jdot_nu = Jdotnu.toNumPy() + 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 = 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 = 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(): + 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, 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 = 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, 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(): + kinDyn2 = idyntree.KinDynComputations() + 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) + 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, joints_val) + assert G_iDyn_np - np.asarray(G_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..60c1c102 --- /dev/null +++ b/tests/mixed/test_CasADi_mixed.py @@ -0,0 +1,241 @@ +# 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 +from adam import Representations + +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(Representations.MIXED_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) +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 + +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(): + 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, 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) + + +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, 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, joints_val)) + CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() + 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) + + +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, 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) + + +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, 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) + + +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, 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, 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) + + +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, 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) + assert p_idy2np - H_test2[: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, 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) + assert p_idy2np - H_test2[: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, 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) + + +def test_coriolis_term(): + g0 = idyntree.Vector3() + g0.zero() + 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, 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(): + base_vel_0 = np.zeros(6) + joints_dot_val_0 = np.zeros(n_dofs) + kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) + 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, 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(): + 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(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/test_CasADi_computations.py b/tests/mixed/test_Jax_mixed.py similarity index 71% rename from tests/test_CasADi_computations.py rename to tests/mixed/test_Jax_mixed.py index 26262e42..636a3391 100644 --- a/tests/test_CasADi_computations.py +++ b/tests/mixed/test_Jax_mixed.py @@ -4,16 +4,19 @@ import logging -import casadi as cs import icub_models import idyntree.swig as idyntree +import jax.numpy as jnp import numpy as np import pytest +from jax import config -from adam.casadi import KinDynComputations +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")) @@ -59,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) @@ -76,50 +80,31 @@ 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(): - 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_)) + mass_test = comp.mass_matrix(H_b, joints_val) 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_)) + 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_f = comp.CoM_position_fun() - CoM_cs = cs.DM(com_f(H_b, s_)) + CoM_test = comp.CoM_position(H_b, joints_val) CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - assert CoM_cs - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) + assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) def test_total_mass(): @@ -129,29 +114,34 @@ def test_total_mass(): 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_)) + 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(): - 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_)) + 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, joints_val, base_vel, joints_dot_val) + Jdotnu = kinDyn.getFrameBiasAcc("l_sole") + Jdot_nu = Jdotnu.toNumPy() + 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) + + 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_)) + 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) @@ -160,8 +150,7 @@ 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_)) + 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) @@ -172,34 +161,34 @@ def test_bias_force(): 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_)) + 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 = comp.coriolis_term_fun() - C_test = cs.DM(C(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) 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) + kinDyn2 = idyntree.KinDynComputations() + 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) + 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 = comp.gravity_term_fun() - G_test = cs.DM(G(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/test_NumPy_computations.py b/tests/mixed/test_NumPy_mixed.py similarity index 79% rename from tests/test_NumPy_computations.py rename to tests/mixed/test_NumPy_mixed.py index 39cbdefb..d4710441 100644 --- a/tests/test_NumPy_computations.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 @@ -75,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) @@ -108,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) @@ -128,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) @@ -136,15 +121,23 @@ 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, joints_val, base_vel, joints_dot_val) + Jdotnu = kinDyn.getFrameBiasAcc("l_sole") + Jdot_nu = Jdotnu.toNumPy() + 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) + + 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) @@ -153,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) @@ -164,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) @@ -186,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) diff --git a/tests/test_pytorch_computations.py b/tests/mixed/test_pytorch_mixed.py similarity index 64% rename from tests/test_pytorch_computations.py rename to tests/mixed/test_pytorch_mixed.py index e4dc7475..de6a8229 100644 --- a/tests/test_pytorch_computations.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,34 +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, joints_val, base_vel, joints_dot_val) + Jdotnu = kinDyn.getFrameBiasAcc("l_sole") + Jdot_nu = Jdotnu.toNumPy() + 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(): @@ -166,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(): @@ -188,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)