diff --git a/.github/workflows/conda-forge-tests.yml b/.github/workflows/conda-forge-tests.yml index 3dff8597..3dab15bc 100644 --- a/.github/workflows/conda-forge-tests.yml +++ b/.github/workflows/conda-forge-tests.yml @@ -25,11 +25,20 @@ jobs: shell: bash -l {0} id: week run: echo "week=$(date +%Y-%U)" >> "${GITHUB_OUTPUT}" + - uses: mamba-org/setup-micromamba@v1 + if: contains(matrix.os, 'macos') || contains(matrix.os, 'ubuntu') with: environment-file: ci_env.yml cache-environment-key: environment-${{ steps.week.outputs.week }}-${{ matrix.os }} + + - uses: mamba-org/setup-micromamba@v1 + if: contains(matrix.os, 'windows') + with: + environment-file: ci_env_win.yml + cache-environment-key: environment-${{ steps.week.outputs.week }}-${{ matrix.os }} + - name: Print packages and environment shell: bash -l {0} run: | @@ -46,8 +55,6 @@ jobs: shell: bash -l {0} if: contains(matrix.os, 'macos') || contains(matrix.os, 'ubuntu') run: | - # Add additional dependencies not available on Windows - micromamba install jax pytorch pytest - name: Test with pytest [Windows] diff --git a/ci_env.yml b/ci_env.yml index 114c442f..50ae6062 100644 --- a/ci_env.yml +++ b/ci_env.yml @@ -16,3 +16,6 @@ dependencies: - pytest-repeat - icub-models - idyntree + - gitpython + - jax + - pytorch \ No newline at end of file diff --git a/ci_env_win.yml b/ci_env_win.yml new file mode 100644 index 00000000..d8a682ca --- /dev/null +++ b/ci_env_win.yml @@ -0,0 +1,19 @@ +name: adamdev +channels: + - conda-forge +dependencies: + - python >=3.7 + - numpy + - scipy + - casadi + - prettytable + - urdfdom-py + - pip + - wheel + - setuptools + - setuptools_scm + - pytest + - pytest-repeat + - icub-models + - idyntree + - gitpython diff --git a/setup.cfg b/setup.cfg index 95729eae..b457f256 100644 --- a/setup.cfg +++ b/setup.cfg @@ -53,6 +53,7 @@ test = idyntree icub-models black + gitpython all = jax jaxlib diff --git a/src/adam/casadi/casadi_like.py b/src/adam/casadi/casadi_like.py index b3372f07..1ea46f07 100644 --- a/src/adam/casadi/casadi_like.py +++ b/src/adam/casadi/casadi_like.py @@ -21,9 +21,9 @@ class CasadiLike(ArrayLike): def __matmul__(self, other: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike": """Overrides @ operator""" if type(other) in [CasadiLike, NumpyLike]: - return CasadiLike(self.array @ other.array) + return CasadiLike(cs.mtimes(self.array, other.array)) else: - return CasadiLike(self.array @ other) + return CasadiLike(cs.mtimes(self.array, other)) def __rmatmul__(self, other: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike": """Overrides @ operator""" @@ -203,6 +203,15 @@ def vertcat(*x) -> "CasadiLike": return CasadiLike(cs.vertcat(*y)) @staticmethod + def horzcat(*x) -> "CasadiLike": + """ + Returns: + CasadiLike: horizontal concatenation of elements + """ + + y = [xi.array if isinstance(xi, CasadiLike) else xi for xi in x] + return CasadiLike(cs.horzcat(*y)) + def solve(A: "CasadiLike", b: "CasadiLike") -> "CasadiLike": """ Args: @@ -215,6 +224,7 @@ def solve(A: "CasadiLike", b: "CasadiLike") -> "CasadiLike": return CasadiLike(cs.solve(A.array, b.array)) + if __name__ == "__main__": math = SpatialMath() print(math.eye(3)) diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py index 72db78d9..0bfdd089 100644 --- a/src/adam/core/spatial_math.py +++ b/src/adam/core/spatial_math.py @@ -117,6 +117,21 @@ def vertcat(x: npt.ArrayLike) -> npt.ArrayLike: """ pass + @abc.abstractmethod + def horzcat(x: npt.ArrayLike) -> npt.ArrayLike: + """ + Args: + x (npt.ArrayLike): elements + + Returns: + npt.ArrayLike: horizontal concatenation of elements x + """ + pass + + @abc.abstractmethod + def mtimes(x: npt.ArrayLike, y: npt.ArrayLike) -> npt.ArrayLike: + pass + @abc.abstractmethod def sin(x: npt.ArrayLike) -> npt.ArrayLike: """ @@ -239,7 +254,9 @@ def H_revolute_joint( T = self.factory.eye(4) R = self.R_from_RPY(rpy) @ self.R_from_axis_angle(axis, q) T[:3, :3] = R - T[:3, 3] = xyz + T[0, 3] = xyz[0] + T[1, 3] = xyz[1] + T[2, 3] = xyz[2] return T def H_prismatic_joint( @@ -276,7 +293,9 @@ def H_from_Pos_RPY(self, xyz: npt.ArrayLike, rpy: npt.ArrayLike) -> npt.ArrayLik """ T = self.factory.eye(4) T[:3, :3] = self.R_from_RPY(rpy) - T[:3, 3] = xyz + T[0, 3] = xyz[0] + T[1, 3] = xyz[1] + T[2, 3] = xyz[2] return T def R_from_RPY(self, rpy: npt.ArrayLike) -> npt.ArrayLike: @@ -393,6 +412,33 @@ def spatial_inertia( IO[:3, :3] = self.factory.eye(3) * mass return IO + def spatial_inertial_with_parameters(self, I, mass, c, rpy): + """ + Args: + I (npt.ArrayLike): inertia values parametric + mass (npt.ArrayLike): mass value parametric + c (npt.ArrayLike): origin of the link parametric + rpy (npt.ArrayLike): orientation of the link from urdf + + Returns: + npt.ArrayLike: the 6x6 inertia matrix parametric expressed at the origin of the link (with rotation) + """ + IO = self.factory.zeros(6, 6) + Sc = self.skew(c) + R = self.factory.zeros(3, 3) + R_temp = self.R_from_RPY(rpy) + inertia_matrix = self.vertcat( + self.horzcat(I.ixx, I.ixy, I.ixz), + self.horzcat(I.iyx, I.iyy, I.iyz), + self.horzcat(I.ixz, I.iyz, I.izz), + ) + + IO[3:, 3:] = R_temp @ inertia_matrix @ R_temp.T + mass * Sc @ Sc.T + IO[3:, :3] = mass * Sc + IO[:3, 3:] = mass * Sc.T + IO[:3, :3] = self.factory.eye(3) * mass + return IO + def spatial_skew(self, v: npt.ArrayLike) -> npt.ArrayLike: """ Args: diff --git a/src/adam/jax/jax_like.py b/src/adam/jax/jax_like.py index c0f18bd3..06aff4bb 100644 --- a/src/adam/jax/jax_like.py +++ b/src/adam/jax/jax_like.py @@ -201,9 +201,21 @@ def vertcat(*x) -> "JaxLike": JaxLike: Vertical concatenation of elements """ if isinstance(x[0], JaxLike): - v = jnp.vstack([x[i].array for i in range(len(x))]).reshape(-1, 1) + v = jnp.vstack([x[i].array for i in range(len(x))]) else: - v = jnp.vstack([x[i] for i in range(len(x))]).reshape(-1, 1) + v = jnp.vstack([x[i] for i in range(len(x))]) + return JaxLike(v) + + @staticmethod + def horzcat(*x) -> "JaxLike": + """ + Returns: + JaxLike: Horizontal concatenation of elements + """ + if isinstance(x[0], JaxLike): + v = jnp.hstack([x[i].array for i in range(len(x))]) + else: + v = jnp.hstack([x[i] for i in range(len(x))]) return JaxLike(v) @staticmethod diff --git a/src/adam/numpy/numpy_like.py b/src/adam/numpy/numpy_like.py index 3c0f05d2..c0f26a31 100644 --- a/src/adam/numpy/numpy_like.py +++ b/src/adam/numpy/numpy_like.py @@ -190,9 +190,21 @@ def vertcat(*x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike": NumpyLike: vertical concatenation of x """ if isinstance(x[0], NumpyLike): - v = np.vstack([x[i].array for i in range(len(x))]).reshape(-1, 1) + v = np.vstack([x[i].array for i in range(len(x))]) else: - v = np.vstack([x[i] for i in range(len(x))]).reshape(-1, 1) + v = np.vstack([x[i] for i in range(len(x))]) + return NumpyLike(v) + + @staticmethod + def horzcat(*x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike": + """ + Returns: + NumpyLike: horrizontal concatenation of x + """ + if isinstance(x[0], NumpyLike): + v = np.hstack([x[i].array for i in range(len(x))]) + else: + v = np.hstack([x[i] for i in range(len(x))]) return NumpyLike(v) @staticmethod diff --git a/src/adam/parametric/__init__.py b/src/adam/parametric/__init__.py new file mode 100644 index 00000000..6f130a0d --- /dev/null +++ b/src/adam/parametric/__init__.py @@ -0,0 +1,3 @@ +# 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. diff --git a/src/adam/parametric/casadi/__init__.py b/src/adam/parametric/casadi/__init__.py new file mode 100644 index 00000000..46a0a0ac --- /dev/null +++ b/src/adam/parametric/casadi/__init__.py @@ -0,0 +1,4 @@ +# 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 .computations_parametric import KinDynComputationsParametric diff --git a/src/adam/parametric/casadi/computations_parametric.py b/src/adam/parametric/casadi/computations_parametric.py new file mode 100644 index 00000000..7e1e8aaa --- /dev/null +++ b/src/adam/parametric/casadi/computations_parametric.py @@ -0,0 +1,275 @@ +# 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.core.constants import Representations +from adam.model import Model +from adam.parametric.model import URDFParametricModelFactory + + +class KinDynComputationsParametric: + """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. This is parametric w.r.t the link length and denisties + """ + + def __init__( + self, + urdfstring: str, + joints_name_list: list, + links_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 + links_name_list (list): list of the parametrized links + root_link (str, optional): the first link. Defaults to 'root_link'. + """ + math = SpatialMath() + n_param_links = len(links_name_list) + self.densities = cs.SX.sym("densities", n_param_links) + self.length_multiplier = cs.SX.sym("length_multiplier", n_param_links) + factory = URDFParametricModelFactory( + path=urdfstring, + math=math, + links_name_list=links_name_list, + length_multiplier=self.length_multiplier, + densities=self.densities, + ) + model = Model.build(factory=factory, joints_name_list=joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=math) + self.NDoF = self.rbdalgos.NDoF + 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, self.length_multiplier, self.densities], + [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, self.length_multiplier, self.densities], + [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, self.length_multiplier, self.densities], + [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, self.length_multiplier, self.densities], + [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, self.length_multiplier, self.densities], [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, + self.length_multiplier, + self.densities, + ], + [J_dot.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, self.length_multiplier, self.densities], + [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, self.length_multiplier, self.densities], + [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, self.length_multiplier, self.densities], + [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, self.length_multiplier, self.densities], + [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, self.length_multiplier, self.densities + ) + + def get_total_mass(self) -> float: + """Returns the total mass of the robot + + Returns: + mass: The total mass + """ + m = self.rbdalgos.get_total_mass() + return cs.Function( + "m", [self.densities, self.length_multiplier], [m], self.f_opts + ) + return self.rbdalgos.get_total_mass() diff --git a/src/adam/parametric/jax/__init__.py b/src/adam/parametric/jax/__init__.py new file mode 100644 index 00000000..04b2fc09 --- /dev/null +++ b/src/adam/parametric/jax/__init__.py @@ -0,0 +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 .computations_parametric import KinDynComputationsParametric diff --git a/src/adam/parametric/jax/computations_parametric.py b/src/adam/parametric/jax/computations_parametric.py new file mode 100644 index 00000000..fb0823c9 --- /dev/null +++ b/src/adam/parametric/jax/computations_parametric.py @@ -0,0 +1,448 @@ +# 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 jax.numpy as jnp +import numpy as np +from jax import grad, jit, vmap + +from adam.core.rbd_algorithms import RBDAlgorithms +from adam.core.constants import Representations +from adam.jax.jax_like import SpatialMath +from adam.model import Model +from adam.parametric.model import URDFParametricModelFactory + + +class KinDynComputationsParametric: + """This is a small class that retrieves robot quantities using Jax + in mixed representation, for Floating Base systems - as humanoid robots. This is parametric w.r.t the link length and denisties + """ + + def __init__( + self, + urdfstring: str, + joints_name_list: list, + links_name_list: list, + root_link: str = "root_link", + gravity: np.array = jnp.array([0, 0, -9.80665, 0, 0, 0]), + ) -> None: + """ + Args: + urdfstring (str): path of the urdf + joints_name_list (list): list of the actuated joints + links_name_list (list): list of parametric links + root_link (str, optional): the first link. Defaults to 'root_link'. + """ + self.math = SpatialMath() + self.links_name_list = links_name_list + self.g = gravity + self.urdfstring = urdfstring + self.joints_name_list = joints_name_list + self.representation = Representations.MIXED_REPRESENTATION # Default + + 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.representation = representation + + def mass_matrix( + self, + base_transform: jnp.array, + joint_positions: jnp.array, + length_multiplier: jnp.array, + densities: jnp.array, + ): + """Returns the Mass Matrix functions computed the CRBA + + Args: + base_transform (jnp.array): The homogenous transform from base to world frame + joint_positions (jnp.array): The joints position + length_multiplier (jnp.array): The length multiplier of the parametrized links + densities (jnp.array): The densities of the parametrized links + + Returns: + M (jnp.array): Mass Matrix + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + [M, _] = self.rbdalgos.crba(base_transform, joint_positions) + return M.array + + def centroidal_momentum_matrix( + self, + base_transform: jnp.array, + joint_positions: jnp.array, + length_multiplier: jnp.array, + densities: jnp.array, + ): + """Returns the Centroidal Momentum Matrix functions computed the CRBA + + Args: + base_transform (jnp.array): The homogenous transform from base to world frame + joint_positions (jnp.array): The joints position + length_multiplier (jnp.array): The length multiplier of the parametrized links + densities (jnp.array): The densities of the parametrized links + + Returns: + Jcc (jnp.array): Centroidal Momentum matrix + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + [_, Jcm] = self.rbdalgos.crba(base_transform, joint_positions) + return Jcm.array + + def relative_jacobian( + self, + frame: str, + joint_positions: jnp.array, + length_multiplier: jnp.array, + densities: jnp.array, + ): + """Returns the Jacobian between the root link and a specified frame frames + + Args: + frame (str): The tip of the chain + joint_positions (jnp.array): The joints position + length_multiplier (jnp.array): The length multiplier of the parametrized links + densities (jnp.array): The densities of the parametrized links + + Returns: + J (jnp.array): The Jacobian between the root and the frame + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + 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, + length_multiplier: jnp.array, + densities: 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 + length_multiplier (jnp.array): The length multiplier of the parametrized links + densities (jnp.array): The densities of the parametrized links + + Returns: + Jdot (jnp.array): The Jacobian derivative relative to the frame + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + 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, + length_multiplier: jnp.array, + densities: jnp.array, + ): + """Computes the forward kinematics relative to the specified frame + + Args: + frame (str): The frame to which the fk will be computed + base_transform (jnp.array): The homogenous transform from base to world frame + joint_positions (jnp.array): The joints position + length_multiplier (jnp.array): The length multiplier of the parametrized links + densities (jnp.array): The densities of the parametrized links + + Returns: + T_fk (jnp.array): The fk represented as Homogenous transformation matrix + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + return self.rbdalgos.forward_kinematics( + frame, base_transform, joint_positions + ).array + + def forward_kinematics_fun( + self, frame, length_multiplier: jnp.array, densities: jnp.array + ): + return lambda T, joint_positions: self.forward_kinematics( + frame, T, joint_positions + ) + + def jacobian( + self, + frame: str, + base_transform, + joint_positions, + length_multiplier: jnp.array, + densities: jnp.array, + ): + """Returns the Jacobian 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 + s (jnp.array): The joints position + length_multiplier (jnp.array): The length multiplier of the parametrized links + densities (jnp.array): The densities of the parametrized links + + Returns: + J_tot (jnp.array): The Jacobian relative to the frame + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + return self.rbdalgos.jacobian(frame, base_transform, joint_positions).array + + def bias_force( + self, + base_transform: jnp.array, + joint_positions: jnp.array, + base_velocity: jnp.array, + s_dot: jnp.array, + length_multiplier: jnp.array, + densities: jnp.array, + ) -> jnp.array: + """Returns the bias force of the floating-base dynamics ejoint_positionsuation, + 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 + length_multiplier (jnp.array): The length multiplier of the parametrized links + densities (jnp.array): The densities of the parametrized links + + + Returns: + h (jnp.array): the bias force + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + return self.rbdalgos.rnea( + base_transform, joint_positions, base_velocity, s_dot, self.g + ).array.squeeze() + + def coriolis_term( + self, + base_transform: jnp.array, + joint_positions: jnp.array, + base_velocity: jnp.array, + s_dot: jnp.array, + length_multiplier: jnp.array, + densities: jnp.array, + ) -> jnp.array: + """Returns the coriolis term of the floating-base dynamics ejoint_positionsuation, + 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 + length_multiplier (jnp.array): The length multiplier of the parametrized links + densities (jnp.array): The densities of the parametrized links + + Returns: + C (jnp.array): the Coriolis term + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + return self.rbdalgos.rnea( + base_transform, + joint_positions, + base_velocity.reshape(6, 1), + s_dot, + np.zeros(6), + ).array.squeeze() + + def gravity_term( + self, + base_transform: jnp.array, + joint_positions: jnp.array, + length_multiplier: jnp.array, + densities: jnp.array, + ) -> jnp.array: + """Returns the gravity term of the floating-base dynamics ejoint_positionsuation, + 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 + length_multiplier (jnp.array): The length multiplier of the parametrized links + densities (jnp.array): The densities of the parametrized links + + Returns: + G (jnp.array): the gravity term + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + return self.rbdalgos.rnea( + base_transform, + joint_positions, + np.zeros(6).reshape(6, 1), + np.zeros(self.NDoF), + self.g, + ).array.squeeze() + + def CoM_position( + self, + base_transform: jnp.array, + joint_positions: jnp.array, + length_multiplier: jnp.array, + densities: jnp.array, + ) -> jnp.array: + """Returns the CoM positon + + Args: + base_transform (jnp.array): The homogenous transform from base to world frame + joint_positions (jnp.array): The joints position + length_multiplier (jnp.array): The length multiplier of the parametrized links + densities (jnp.array): The densities of the parametrized links + + Returns: + com (jnp.array): The CoM position + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + return self.rbdalgos.CoM_position( + base_transform, joint_positions + ).array.squeeze() + + def get_total_mass( + self, length_multiplier: jnp.array, densities: jnp.array + ) -> float: + """Returns the total mass of the robot + + Args: + length_multiplier (jnp.array): The length multiplier of the parametrized links + densities (jnp.array): The densities of the parametrized links + + Returns: + mass: The total mass + """ + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + return self.rbdalgos.get_total_mass() diff --git a/src/adam/parametric/model/__init__.py b/src/adam/parametric/model/__init__.py new file mode 100644 index 00000000..729aec3e --- /dev/null +++ b/src/adam/parametric/model/__init__.py @@ -0,0 +1,7 @@ +# 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 .parametric_factories.parametric_joint import ParmetricJoint +from .parametric_factories.parametric_link import ParametricLink +from .parametric_factories.parametric_model import URDFParametricModelFactory diff --git a/src/adam/parametric/model/parametric_factories/__init__.py b/src/adam/parametric/model/parametric_factories/__init__.py new file mode 100644 index 00000000..6f130a0d --- /dev/null +++ b/src/adam/parametric/model/parametric_factories/__init__.py @@ -0,0 +1,3 @@ +# 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. diff --git a/src/adam/parametric/model/parametric_factories/parametric_joint.py b/src/adam/parametric/model/parametric_factories/parametric_joint.py new file mode 100644 index 00000000..302b755b --- /dev/null +++ b/src/adam/parametric/model/parametric_factories/parametric_joint.py @@ -0,0 +1,144 @@ +from typing import Union + +import numpy.typing as npt +import urdf_parser_py.urdf + +from adam.core.spatial_math import SpatialMath +from adam.model import Joint +from adam.parametric.model.parametric_factories.parametric_link import ParametricLink + + +class ParmetricJoint(Joint): + """Parametric Joint class""" + + def __init__( + self, + joint: urdf_parser_py.urdf.Joint, + math: SpatialMath, + parent_link: ParametricLink, + idx: Union[int, None] = None, + ) -> None: + self.math = math + self.name = joint.name + self.parent = parent_link.link.name + self.parent_parametric = parent_link + self.child = joint.child + self.type = joint.joint_type + self.axis = joint.axis + self.limit = joint.limit + self.idx = idx + self.joint = joint + joint_offset = self.parent_parametric.compute_joint_offset( + joint, self.parent_parametric.link_offset + ) + self.offset = joint_offset + self.origin = self.modify(self.parent_parametric.link_offset) + + def modify(self, parent_joint_offset: npt.ArrayLike): + """ + Args: + parent_joint_offset (npt.ArrayLike): offset of the parent joint + + Returns: + npt.ArrayLike: the origin of the joint, parametric with respect to the parent link dimensions + """ + + length = self.parent_parametric.get_principal_lenght_parametric() + # Ack for avoiding depending on casadi + vo = self.parent_parametric.origin[2] + xyz_rpy = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + xyz_rpy[0] = self.joint.origin.xyz[0] + xyz_rpy[1] = self.joint.origin.xyz[1] + xyz_rpy[2] = self.joint.origin.xyz[2] + xyz_rpy[3] = self.joint.origin.rpy[0] + xyz_rpy[4] = self.joint.origin.rpy[1] + xyz_rpy[5] = self.joint.origin.rpy[2] + + if xyz_rpy[2] < 0: + xyz_rpy[2] = -length + parent_joint_offset - self.offset + else: + xyz_rpy[2] = vo + length / 2 - self.offset + return xyz_rpy + + def homogeneous(self, q: npt.ArrayLike) -> npt.ArrayLike: + """ + Args: + q (npt.ArrayLike): joint value + + Returns: + npt.ArrayLike: the homogenous transform of a joint, given q + """ + + o = self.math.factory.zeros(3) + o[0] = self.origin[0] + o[1] = self.origin[1] + o[2] = self.origin[2] + rpy = self.origin[3:] + + if self.type == "fixed": + return self.math.H_from_Pos_RPY(o, rpy) + elif self.type in ["revolute", "continuous"]: + return self.math.H_revolute_joint( + o, + rpy, + self.axis, + q, + ) + elif self.type in ["prismatic"]: + return self.math.H_prismatic_joint( + o, + rpy, + self.axis, + q, + ) + + def spatial_transform(self, q: npt.ArrayLike) -> npt.ArrayLike: + """ + Args: + q (npt.ArrayLike): joint motion + + Returns: + npt.ArrayLike: spatial transform of the joint given q + """ + if self.type == "fixed": + return self.math.X_fixed_joint(self.origin[:3], self.origin[3:]) + elif self.type in ["revolute", "continuous"]: + return self.math.X_revolute_joint( + self.origin[:3], self.origin[3:], self.axis, q + ) + elif self.type in ["prismatic"]: + return self.math.X_prismatic_joint( + self.origin[:3], + self.origin[3:], + self.axis, + q, + ) + + def motion_subspace(self) -> npt.ArrayLike: + """ + Args: + joint (Joint): Joint + + Returns: + npt.ArrayLike: motion subspace of the joint + """ + if self.type == "fixed": + return self.math.vertcat(0, 0, 0, 0, 0, 0) + elif self.type in ["revolute", "continuous"]: + return self.math.vertcat( + 0, + 0, + 0, + self.axis[0], + self.axis[1], + self.axis[2], + ) + elif self.type in ["prismatic"]: + return self.math.vertcat( + self.axis[0], + self.axis[1], + self.axis[2], + 0, + 0, + 0, + ) diff --git a/src/adam/parametric/model/parametric_factories/parametric_link.py b/src/adam/parametric/model/parametric_factories/parametric_link.py new file mode 100644 index 00000000..389e417b --- /dev/null +++ b/src/adam/parametric/model/parametric_factories/parametric_link.py @@ -0,0 +1,295 @@ +import numpy.typing as npt +import urdf_parser_py.urdf +from enum import Enum + +from adam.core.spatial_math import SpatialMath +from adam.model import Link + +import math +from adam.model.abc_factories import Inertial + + +class I_parametric: + """Inertia values parametric""" + + def __init__(self) -> None: + self.ixx = 0.0 + self.ixy = 0.0 + self.ixz = 0.0 + self.iyx = 0.0 + self.iyy = 0.0 + self.iyz = 0.0 + self.izz = 0.0 + self.ixz = 0.0 + + +class Geometry(Enum): + """The different types of geometries that constitute the URDF""" + + BOX = 1 + CYLINDER = 2 + SPHERE = 3 + + +class Side(Enum): + """The possible sides of a box geometry""" + + WIDTH = 1 + HEIGHT = 2 + DEPTH = 3 + + +class ParametricLink(Link): + """Parametric Link class""" + + def __init__( + self, + link: urdf_parser_py.urdf.Link, + math: SpatialMath, + length_multiplier, + densities, + ): + self.math = math + self.name = link.name + self.length_multiplier = length_multiplier + self.densities = densities + self.visuals = link.visual + self.geometry_type, self.visual_data = self.get_geometry(self.visuals) + self.link = link + self.link_offset = self.compute_offset() + (self.volume, self.visual_data_new) = self.compute_volume() + self.mass = self.compute_mass() + self.I = self.compute_inertia_parametric() + self.origin = self.modify_origin() + self.inertial = Inertial(self.mass) + self.inertial.mass = self.mass + self.inertial.inertia = self.I + self.inertial.origin = self.origin + + def get_principal_lenght(self): + """Method computing the principal link length, i.e. the dimension in which the kinematic chain grows""" + xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] + if self.geometry_type == Geometry.CYLINDER: + if xyz_rpy[3] < 0.0 or xyz_rpy[4] > 0.0: + v_l = ( + 2 * self.visual_data.radius + ) # returning the diameter, since the orientation of the shape is such that the radius is the principal lenght + else: + v_l = ( + self.visual_data.length + ) # returning the lenght, since the orientation of the shape is such that the radius is the principal lenght + elif self.geometry_type == Geometry.SPHERE: + v_l = self.visual_data.radius + elif self.geometry_type == Geometry.BOX: + v_l = self.visual_data.size[2] + else: + raise Exception(f"THE GEOMETRY IS NOT SPECIFIED") + return v_l + + def get_principal_lenght_parametric(self): + """Method computing the principal link length parametric, i.e. the dimension in which the kinematic chain grows""" + xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] + if self.geometry_type == Geometry.CYLINDER: + if xyz_rpy[3] < 0.0 or xyz_rpy[4] > 0.0: + v_l = ( + 2 * self.visual_data_new[1] + ) # returning the diameter, since the orientation of the shape is such that the radius is the principal lenght + else: + v_l = self.visual_data_new[ + 0 + ] # returning the lenght, since the orientation of the shape is such that the radius is the principal lenght + elif self.geometry_type == Geometry.SPHERE: + v_l = self.visual_data_new + elif self.geometry_type == Geometry.BOX: + v_l = self.visual_data_new[2] + else: + raise Exception(f"THE GEOMETRY IS NOT SPECIFIED") + return v_l + + def compute_offset(self): + """ + Returns: + npt.ArrayLike: link offset + """ + xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] + v_l = self.get_principal_lenght() + v_o = xyz_rpy[2] + if v_o < 0: + link_offset = v_l / 2 + v_o + else: + link_offset = v_o - v_l / 2 + return link_offset + + def compute_joint_offset(self, joint_i, parent_offset): + """ + Returns: + npt.ArrayLike: the child joint offset + """ + # Taking the principal direction i.e. the length + xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] + v_l = self.get_principal_lenght() + j_0 = joint_i.origin.xyz[2] + v_o = xyz_rpy[2] + if j_0 < 0: + joint_offset_temp = -(v_l + j_0 - parent_offset) + joint_offset = joint_offset_temp + else: + joint_offset_temp = v_l + parent_offset - j_0 + joint_offset = joint_offset_temp + return joint_offset + + @staticmethod + def get_geometry(visual_obj): + """ + Returns: + (Geometry, urdf geometry): the geometry of the link and the related urdf object + """ + if type(visual_obj.geometry) is urdf_parser_py.urdf.Box: + return (Geometry.BOX, visual_obj.geometry) + if type(visual_obj.geometry) is urdf_parser_py.urdf.Cylinder: + return (Geometry.CYLINDER, visual_obj.geometry) + if type(visual_obj.geometry) is urdf_parser_py.urdf.Sphere: + return (Geometry.SPHERE, visual_obj.geometry) + + def compute_volume(self): + """ + Returns: + (npt.ArrayLike, npt.ArrayLike): the volume and the dimension parametric + """ + volume = 0.0 + """Modifies a link's volume by a given multiplier, in a manner that is logical with the link's geometry""" + if self.geometry_type == Geometry.BOX: + visual_data_new = [0.0, 0.0, 0.0] + visual_data_new[0] = self.visual_data.size[0] # * self.length_multiplier[0] + visual_data_new[1] = self.visual_data.size[1] # * self.length_multiplier[1] + visual_data_new[2] = self.visual_data.size[2] * self.length_multiplier + volume = visual_data_new[0] * visual_data_new[1] * visual_data_new[2] + elif self.geometry_type == Geometry.CYLINDER: + visual_data_new = [0.0, 0.0] + visual_data_new[0] = self.visual_data.length * self.length_multiplier + visual_data_new[1] = self.visual_data.radius # * self.length_multiplier[1] + volume = math.pi * visual_data_new[1] ** 2 * visual_data_new[0] + elif self.geometry_type == Geometry.SPHERE: + visual_data_new = 0.0 + visual_data_new = self.visual_data.radius * self.length_multiplier + volume = 4 * math.pi * visual_data_new**3 / 3 + return volume, visual_data_new + + """Function that computes the mass starting from the densities, the length multiplier and the link""" + + def compute_mass(self): + """ + Returns: + (npt.ArrayLike): the link mass + """ + mass = 0.0 + mass = self.volume * self.densities + return mass + + def modify_origin(self): + """ + Returns: + (npt.ArrayLike): the link origin parametrized + """ + origin = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] + v_o = xyz_rpy[2] + length = self.get_principal_lenght_parametric() + if v_o < 0: + origin[2] = self.link_offset - length / 2 + origin[0] = xyz_rpy[0] + origin[1] = xyz_rpy[1] + origin[3] = xyz_rpy[3] + origin[4] = xyz_rpy[4] + origin[5] = xyz_rpy[5] + else: + origin[2] = length / 2 + self.link_offset + origin[0] = xyz_rpy[0] + origin[1] = xyz_rpy[1] + origin[3] = xyz_rpy[3] + origin[4] = xyz_rpy[4] + origin[5] = xyz_rpy[5] + if self.geometry_type == Geometry.SPHERE: + "in case of a sphere the origin of the framjoint_name_list[0]:link_parametric.JointCharacteristics(0.0295),e does not change" + origin = xyz_rpy + return origin + + def compute_inertia_parametric(self): + """ + Returns: + Inertia Parametric: inertia (ixx, iyy and izz) with the formula that corresponds to the geometry + Formulas retrieved from https://en.wikipedia.org/wiki/List_of_moments_of_inertia + """ + I = I_parametric() + xyz_rpy = [*self.visuals.origin.xyz, *self.visuals.origin.rpy] + if self.geometry_type == Geometry.BOX: + I.ixx = ( + self.mass + * (self.visual_data_new[1] ** 2 + self.visual_data_new[2] ** 2) + / 12 + ) + I.iyy = ( + self.mass + * (self.visual_data_new[0] ** 2 + self.visual_data_new[2] ** 2) + / 12 + ) + I.izz = ( + self.mass + * (self.visual_data_new[0] ** 2 + self.visual_data_new[1] ** 2) + / 12 + ) + elif self.geometry_type == Geometry.CYLINDER: + i_xy_incomplete = ( + 3 * (self.visual_data_new[1] ** 2) + self.visual_data_new[0] ** 2 + ) / 12 + I.ixx = self.mass * i_xy_incomplete + I.iyy = self.mass * i_xy_incomplete + I.izz = self.mass * self.visual_data_new[1] ** 2 / 2 + + if xyz_rpy[3] > 0 and xyz_rpy[4] == 0.0 and xyz_rpy[5] == 0.0: + itemp = I.izz + I.iyy = itemp + I.izz = I.ixx + elif xyz_rpy[4] > 0.0: + itemp = I.izz + I.ixx = itemp + I.izz = I.iyy + return I + elif self.geometry_type == Geometry.SPHERE: + I.ixx = 2 * self.mass * self.visual_data_new**2 / 5 + I.iyy = I.ixx + I.izz = I.ixx + return I + + def spatial_inertia(self) -> npt.ArrayLike: + """ + Args: + link (Link): Link + + Returns: + npt.ArrayLike: the 6x6 inertia matrix expressed at the origin of the link (with rotation) + """ + I = self.I + mass = self.mass + o = self.math.factory.zeros(3) + o[0] = self.origin[0] + o[1] = self.origin[1] + o[2] = self.origin[2] + rpy = self.origin[3:] + return self.math.spatial_inertial_with_parameters(I, mass, o, rpy) + + def homogeneous(self) -> npt.ArrayLike: + """ + Returns: + npt.ArrayLike: the homogeneus transform of the link + """ + + o = self.math.factory.zeros(3) + o[0] = self.origin[0] + o[1] = self.origin[1] + o[2] = self.origin[2] + rpy = self.origin[3:] + return self.math.H_from_Pos_RPY( + o, + rpy, + ) diff --git a/src/adam/parametric/model/parametric_factories/parametric_model.py b/src/adam/parametric/model/parametric_factories/parametric_model.py new file mode 100644 index 00000000..1a460944 --- /dev/null +++ b/src/adam/parametric/model/parametric_factories/parametric_model.py @@ -0,0 +1,115 @@ +import pathlib +from typing import List + +import urdf_parser_py.urdf + +from adam.core.spatial_math import SpatialMath +from adam.model import ModelFactory, StdJoint, StdLink, Link, Joint +from adam.parametric.model import ParmetricJoint, ParametricLink + + +class URDFParametricModelFactory(ModelFactory): + """This factory generates robot elements from urdf_parser_py parametrized w.r.t. link lengths and densities + + Args: + ModelFactory: the Model factory + """ + + def __init__( + self, + path: str, + math: SpatialMath, + links_name_list: List, + length_multiplier, + densities, + ): + self.math = math + if type(path) is not pathlib.Path: + path = pathlib.Path(path) + if not path.exists(): + raise FileExistsError(path) + self.links_name_list = links_name_list + self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_file(path) + self.name = self.urdf_desc.name + self.length_multiplier = length_multiplier + self.densities = densities + + def get_joints(self) -> List[Joint]: + """ + Returns: + List[Joint]: build the list of the joints + """ + return [self.build_joint(j) for j in self.urdf_desc.joints] + + def get_links(self) -> List[Link]: + """ + Returns: + List[Link]: build the list of the links + """ + return [ + self.build_link(l) for l in self.urdf_desc.links if l.inertial is not None + ] + + def get_frames(self) -> List[StdLink]: + """ + Returns: + List[Link]: build the list of the links + """ + return [self.build_link(l) for l in self.urdf_desc.links if l.inertial is None] + + def build_joint(self, joint: urdf_parser_py.urdf.Joint) -> Joint: + """ + Args: + joint (Joint): the urdf_parser_py joint + + Returns: + StdJoint/ParametricJoint: our joint representation + """ + if joint.parent in self.links_name_list: + index_link = self.links_name_list.index(joint.parent) + link_parent = self.get_element_by_name(joint.parent) + parent_link_parametric = ParametricLink( + link_parent, + self.math, + self.length_multiplier[index_link], + self.densities[index_link], + ) + return ParmetricJoint(joint, self.math, parent_link_parametric) + + return StdJoint(joint, self.math) + + def build_link(self, link: urdf_parser_py.urdf.Link) -> Link: + """ + Args: + link (Link): the urdf_parser_py link + + Returns: + StdLink/ParametricLink: our link representation + """ + if link.name in self.links_name_list: + index_link = self.links_name_list.index(link.name) + return ParametricLink( + link, + self.math, + self.length_multiplier[index_link], + self.densities[index_link], + ) + return StdLink(link, self.math) + + def get_element_by_name(self, link_name): + """ + Args: + link_name (Link): the link name + + Returns: + Link: the urdf parser link object associated to the link name + """ + link_list = [ + corresponding_link + for corresponding_link in self.urdf_desc.links + if corresponding_link.name == link_name + ] + if len(link_list) != 0: + return link_list[0] + else: + return None diff --git a/src/adam/parametric/numpy/__init__.py b/src/adam/parametric/numpy/__init__.py new file mode 100644 index 00000000..04b2fc09 --- /dev/null +++ b/src/adam/parametric/numpy/__init__.py @@ -0,0 +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 .computations_parametric import KinDynComputationsParametric diff --git a/src/adam/parametric/numpy/computations_parametric.py b/src/adam/parametric/numpy/computations_parametric.py new file mode 100644 index 00000000..e4338251 --- /dev/null +++ b/src/adam/parametric/numpy/computations_parametric.py @@ -0,0 +1,443 @@ +# 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 numpy as np + +from adam.core.rbd_algorithms import RBDAlgorithms +from adam.core.constants import Representations +from adam.model import Model +from adam.parametric.model import URDFParametricModelFactory +from adam.numpy.numpy_like import SpatialMath + + +class KinDynComputationsParametric: + """This is a small class that retrieves robot quantities using NumPy + in mixed representation, for Floating Base systems - as humanoid robots. This is parametric w.r.t the link length and denisties + """ + + def __init__( + self, + urdfstring: str, + joints_name_list: list, + links_name_list: list, + root_link: str = "root_link", + gravity: np.array = np.array([0, 0, -9.80665, 0, 0, 0]), + ) -> None: + """ + Args: + urdfstring (str): path of the urdf + joints_name_list (list): list of the actuated joints + links_name_list (list): list of parametric links + root_link (str, optional): the first link. Defaults to 'root_link'. + """ + self.links_name_list = links_name_list + self.math = SpatialMath() + self.g = gravity + self.urdfstring = urdfstring + self.joints_name_list = joints_name_list + self.representation = Representations.MIXED_REPRESENTATION # Default + + 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.representation = representation + + def mass_matrix( + self, + base_transform: np.ndarray, + joint_positions: np.ndarray, + length_multiplier: np.ndarray, + densities: np.ndarray, + ) -> np.ndarray: + """Returns the Mass Matrix functions computed the CRBA + + Args: + base_transform (np.ndarray): The homogenous transform from base to world frame + joint_positions (np.ndarray): The joints position + length_multiplier (np.ndarray): The length multiplier of the parametrized links + densities (np.ndarray): The densities of the parametrized links + + Returns: + M (np.ndarray): Mass Matrix + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = model.NDoF + [M, _] = self.rbdalgos.crba(base_transform, joint_positions) + return M.array + + def centroidal_momentum_matrix( + self, + base_transform: np.ndarray, + s: np.ndarray, + length_multiplier: np.ndarray, + densities: np.ndarray, + ) -> np.ndarray: + """Returns the Centroidal Momentum Matrix functions computed the CRBA + + Args: + base_transform (np.ndarray): The homogenous transform from base to world frame + joint_positions (np.ndarray): The joint positions + length_multiplier (np.ndarray): The length multiplier of the parametrized links + densities (np.ndarray): The densities of the parametrized links + + Returns: + Jcc (np.ndarray): Centroidal Momentum matrix + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = model.NDoF + [_, Jcm] = self.rbdalgos.crba(base_transform, s) + return Jcm.array + + def forward_kinematics( + self, + frame: str, + base_transform: np.ndarray, + joint_positions: np.ndarray, + length_multiplier: np.ndarray, + densities: np.ndarray, + ) -> np.ndarray: + """Computes the forward kinematics relative to the specified frame + + Args: + frame (str): The frame to which the fk will be computed + base_transform (np.ndarray): The homogenous transform from base to world frame + joint_positions (np.ndarray): The joints position + length_multiplier (np.ndarray): The length multiplier of the parametrized links + densities (np.ndarray): The densities of the parametrized links + + Returns: + T_fk (np.ndarray): The fk represented as Homogenous transformation matrix + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = model.NDoF + return self.rbdalgos.forward_kinematics( + frame, base_transform, joint_positions + ).array.squeeze() + + def jacobian( + self, + frame: str, + base_transform: np.ndarray, + joint_positions: np.ndarray, + length_multiplier: np.ndarray, + densities: np.ndarray, + ) -> np.ndarray: + """Returns the Jacobian 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 + length_multiplier (np.ndarray): The length multiplier of the parametrized links + densities (np.ndarray): The densities of the parametrized links + + Returns: + J_tot (np.ndarray): The Jacobian relative to the frame + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = model.NDoF + return self.rbdalgos.jacobian( + frame, base_transform, joint_positions + ).array.squeeze() + + def relative_jacobian( + self, + frame: str, + joint_positions: np.ndarray, + length_multiplier: np.ndarray, + densities: np.ndarray, + ) -> np.ndarray: + """Returns the Jacobian between the root link and a specified frame frames + + Args: + frame (str): The tip of the chain + joint_positions (np.ndarray): The joints position + length_multiplier (np.ndarray): The length multiplier of the parametrized links + densities (np.ndarray): The densities of the parametrized links + + Returns: + J (np.ndarray): The Jacobian between the root and the frame + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = model.NDoF + 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, + length_multiplier: np.ndarray, + densities: 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 + length_multiplier (np.ndarray): The length multiplier of the parametrized links + densities (np.ndarray): The densities of the parametrized links + + Returns: + Jdot (np.ndarray): The Jacobian derivative relative to the frame + """ + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = model.NDoF + 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, + length_multiplier: np.ndarray, + densities: np.ndarray, + ) -> np.ndarray: + """Returns the CoM positon + + Args: + base_transform (np.ndarray): The homogenous transform from base to world frame + joint_positions (np.ndarray): The joints position + length_multiplier (np.ndarray): The length multiplier of the parametrized links + densities (np.ndarray): The densities of the parametrized links + + Returns: + CoM (np.ndarray): The CoM position + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = model.NDoF + return self.rbdalgos.CoM_position( + base_transform, joint_positions + ).array.squeeze() + + def bias_force( + self, + base_transform: np.ndarray, + joint_positions: np.ndarray, + base_velocity: np.ndarray, + joint_velocities: np.ndarray, + length_multiplier: np.ndarray, + densities: np.ndarray, + ) -> np.ndarray: + """Returns the bias force of the floating-base dynamics equation, + using a reduced RNEA (no acceleration and external forces) + + Args: + 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 + length_multiplier (np.ndarray): The length multiplier of the parametrized links + densities (np.ndarray): The densities of the parametrized links + + Returns: + h (np.ndarray): the bias force + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = model.NDoF + return self.rbdalgos.rnea( + base_transform, + joint_positions, + base_velocity.reshape(6, 1), + joint_velocities, + self.g, + ).array.squeeze() + + def coriolis_term( + self, + base_transform: np.ndarray, + joint_positions: np.ndarray, + base_velocity: np.ndarray, + joint_velocities: np.ndarray, + length_multiplier: np.ndarray, + densities: np.ndarray, + ) -> np.ndarray: + """Returns the coriolis term of the floating-base dynamics equation, + using a reduced RNEA (no acceleration and external forces) + + Args: + 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 + length_multiplier (np.ndarray): The length multiplier of the parametrized links + densities (np.ndarray): The densities of the parametrized links + + Returns: + C (np.ndarray): the Coriolis term + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = model.NDoF + # set in the bias force computation the gravity term to zero + return self.rbdalgos.rnea( + base_transform, + joint_positions, + base_velocity.reshape(6, 1), + joint_velocities, + np.zeros(6), + ).array.squeeze() + + def gravity_term( + self, + base_transform: np.ndarray, + joint_positions: np.ndarray, + length_multiplier: np.ndarray, + densities: np.ndarray, + ) -> np.ndarray: + """Returns the gravity term of the floating-base dynamics equation, + using a reduced RNEA (no acceleration and external forces) + + Args: + base_transform (np.ndarray): The homogenous transform from base to world frame + joint_positions (np.ndarray): The joints position + length_multiplier (np.ndarray): The length multiplier of the parametrized links + densities (np.ndarray): The densities of the parametrized links + + Returns: + G (np.ndarray): the gravity term + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = model.NDoF + return self.rbdalgos.rnea( + base_transform, + joint_positions, + np.zeros(6).reshape(6, 1), + np.zeros(self.NDoF), + self.g, + ).array.squeeze() + + def get_total_mass( + self, length_multiplier: np.ndarray, densities: np.ndarray + ) -> float: + """Returns the total mass of the robot + + Args: + length_multiplier (np.ndarray): The length multiplier of the parametrized links + densities (np.ndarray): The densities of the parametrized links + + Returns: + mass: The total mass + """ + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = model.NDoF + return self.rbdalgos.get_total_mass() diff --git a/src/adam/parametric/pytorch/__init__.py b/src/adam/parametric/pytorch/__init__.py new file mode 100644 index 00000000..04b2fc09 --- /dev/null +++ b/src/adam/parametric/pytorch/__init__.py @@ -0,0 +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 .computations_parametric import KinDynComputationsParametric diff --git a/src/adam/parametric/pytorch/computations_parametric.py b/src/adam/parametric/pytorch/computations_parametric.py new file mode 100644 index 00000000..8e53eea1 --- /dev/null +++ b/src/adam/parametric/pytorch/computations_parametric.py @@ -0,0 +1,443 @@ +# 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 numpy as np +import torch + +from adam.core.rbd_algorithms import RBDAlgorithms +from adam.core.constants import Representations +from adam.model import Model, URDFModelFactory +from adam.parametric.model import URDFParametricModelFactory +from adam.pytorch.torch_like import SpatialMath + + +class KinDynComputationsParametric: + """This is a small class that retrieves robot quantities using Pytorch + in mixed representation, for Floating Base systems - as humanoid robots. This is parametric w.r.t the link length and denisties + """ + + def __init__( + self, + urdfstring: str, + joints_name_list: list, + links_name_list: list, + root_link: str = "root_link", + gravity: np.array = torch.FloatTensor([0, 0, -9.80665, 0, 0, 0]), + ) -> None: + """ + Args: + urdfstring (str): path of the urdf + joints_name_list (list): list of the actuated joints + links_name_list (list): list of parametric links + root_link (str, optional): the first link. Defaults to 'root_link'. + """ + self.math = SpatialMath() + self.g = gravity + self.links_name_list = links_name_list + self.joints_name_list = joints_name_list + self.urdfstring = urdfstring + self.representation = Representations.MIXED_REPRESENTATION # Default + + 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.representation = representation + + def mass_matrix( + self, + base_transform: torch.Tensor, + s: torch.Tensor, + length_multiplier: torch.Tensor, + densities: 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 + length_multiplier (torch.tensor): The length multiplier of the parametrized links + densities (torch.tensor): The densities of the parametrized links + + Returns: + M (torch.tensor): Mass Matrix + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + [M, _] = self.rbdalgos.crba(base_transform, s) + return M.array + + def centroidal_momentum_matrix( + self, + base_transform: torch.Tensor, + s: torch.Tensor, + length_multiplier: torch.Tensor, + densities: 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 + length_multiplier (torch.tensor): The length multiplier of the parametrized links + densities (torch.tensor): The densities of the parametrized links + + Returns: + Jcc (torch.tensor): Centroidal Momentum matrix + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + [_, Jcm] = self.rbdalgos.crba(base_transform, s) + return Jcm.array + + def forward_kinematics( + self, + frame, + base_transform: torch.Tensor, + s: torch.Tensor, + length_multiplier: torch.Tensor, + densities: 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 + length_multiplier (torch.tensor): The length multiplier of the parametrized links + densities (torch.tensor): The densities of the parametrized links + + Returns: + T_fk (torch.tensor): The fk represented as Homogenous transformation matrix + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + return ( + self.rbdalgos.forward_kinematics( + frame, torch.FloatTensor(base_transform), torch.FloatTensor(s) + ) + ).array + + def jacobian( + self, + frame: str, + base_transform: torch.Tensor, + joint_positions: torch.Tensor, + length_multiplier: torch.Tensor, + densities: torch.Tensor, + ) -> torch.Tensor: + """Returns the Jacobian 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 + length_multiplier (torch.tensor): The length multiplier of the parametrized links + densities (torch.tensor): The densities of the parametrized links + + Returns: + J_tot (torch.tensor): The Jacobian relative to the frame + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + return self.rbdalgos.jacobian(frame, base_transform, joint_positions).array + + def relative_jacobian( + self, + frame, + joint_positions: torch.Tensor, + length_multiplier: torch.Tensor, + densities: torch.Tensor, + ) -> torch.Tensor: + """Returns the Jacobian between the root link and a specified frame frames + + Args: + frame (str): The tip of the chain + joint_positions (torch.tensor): The joints position + length_multiplier (torch.tensor): The length multiplier of the parametrized links + densities (torch.tensor): The densities of the parametrized links + + Returns: + J (torch.tensor): The Jacobian between the root and the frame + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + 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, + length_multiplier: torch.Tensor, + densities: 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 + length_multiplier (torch.tensor): The length multiplier of the parametrized links + densities (torch.tensor): The densities of the parametrized links + + Returns: + Jdot (torch.Tensor): The Jacobian derivative relative to the frame + """ + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + 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, + length_multiplier: torch.Tensor, + densities: torch.Tensor, + ) -> torch.Tensor: + """Returns the CoM positon + + Args: + base_transform (torch.tensor): The homogenous transform from base to world frame + joint_positions (torch.tensor): The joints position + length_multiplier (torch.tensor): The length multiplier of the parametrized links + densities (torch.tensor): The densities of the parametrized links + + Returns: + com (torch.tensor): The CoM position + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + return self.rbdalgos.CoM_position( + base_transform, joint_positions + ).array.squeeze() + + def bias_force( + self, + base_transform: torch.Tensor, + s: torch.Tensor, + base_velocity: torch.Tensor, + joint_velocities: torch.Tensor, + length_multiplier: torch.Tensor, + densities: torch.Tensor, + ) -> torch.Tensor: + """Returns the bias force 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 + s (torch.tensor): The joints position + base_velocity (torch.tensor): The base velocity in mixed representation + joint_velocities (torch.tensor): The joints velocity + length_multiplier (torch.tensor): The length multiplier of the parametrized links + densities (torch.tensor): The densities of the parametrized links + + Returns: + h (torch.tensor): the bias force + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + return self.rbdalgos.rnea( + base_transform, + s, + base_velocity.reshape(6, 1), + joint_velocities, + self.g, + ).array.squeeze() + + def coriolis_term( + self, + base_transform: torch.Tensor, + joint_positions: torch.Tensor, + base_velocity: torch.Tensor, + joint_velocities: torch.Tensor, + length_multiplier: torch.Tensor, + densities: torch.Tensor, + ) -> torch.Tensor: + """Returns the coriolis 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 + joint_positions (torch.tensor): The joints position + base_velocity (torch.tensor): The base velocity in mixed representation + joint_velocities (torch.tensor): The joints velocity + length_multiplier (torch.tensor): The length multiplier of the parametrized links + densities (torch.tensor): The densities of the parametrized links + + Returns: + C (torch.tensor): the Coriolis term + """ + + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + # set in the bias force computation the gravity term to zero + return self.rbdalgos.rnea( + base_transform, + joint_positions, + base_velocity.reshape(6, 1), + joint_velocities, + torch.zeros(6), + ).array.squeeze() + + def gravity_term( + self, + base_transform: torch.Tensor, + base_positions: torch.Tensor, + length_multiplier: torch.Tensor, + densities: 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 + length_multiplier (torch.tensor): The length multiplier of the parametrized links + densities (torch.tensor): The densities of the parametrized links + + Returns: + G (torch.tensor): the gravity term + """ + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + return self.rbdalgos.rnea( + base_transform, + base_positions, + torch.zeros(6).reshape(6, 1), + torch.zeros(self.NDoF), + torch.FloatTensor(self.g), + ).array.squeeze() + + def get_total_mass( + self, length_multiplier: torch.Tensor, densities: torch.Tensor + ) -> float: + """Returns the total mass of the robot + + Args: + length_multiplier (torch.tensor): The length multiplier of the parametrized links + densities (torch.tensor): The densities of the parametrized links + + Returns: + mass: The total mass + """ + factory = URDFParametricModelFactory( + path=self.urdfstring, + math=self.math, + links_name_list=self.links_name_list, + length_multiplier=length_multiplier, + densities=densities, + ) + model = Model.build(factory=factory, joints_name_list=self.joints_name_list) + self.rbdalgos = RBDAlgorithms(model=model, math=self.math) + self.rbdalgos.set_frame_velocity_representation(self.representation) + self.NDoF = self.rbdalgos.NDoF + return self.rbdalgos.get_total_mass() diff --git a/src/adam/pytorch/torch_like.py b/src/adam/pytorch/torch_like.py index fad7d4fe..4753d45d 100644 --- a/src/adam/pytorch/torch_like.py +++ b/src/adam/pytorch/torch_like.py @@ -23,7 +23,7 @@ def __setitem__(self, idx, value: Union["TorchLike", ntp.ArrayLike]) -> "TorchLi if type(self) is type(value): self.array[idx] = value.array.reshape(self.array[idx].shape) else: - self.array[idx] = torch.FloatTensor(value) + self.array[idx] = torch.tensor(value) def __getitem__(self, idx): """Overrides get item operator""" @@ -216,9 +216,21 @@ def vertcat(*x: ntp.ArrayLike) -> "TorchLike": TorchLike: vertical concatenation of x """ if isinstance(x[0], TorchLike): - v = torch.vstack([x[i].array for i in range(len(x))]).reshape(-1, 1) + v = torch.vstack([x[i].array for i in range(len(x))]) else: - v = torch.FloatTensor(x).reshape(-1, 1) + v = torch.FloatTensor(x) + return TorchLike(v) + + @staticmethod + def horzcat(*x: ntp.ArrayLike) -> "TorchLike": + """ + Returns: + TorchLike: horizontal concatenation of x + """ + if isinstance(x[0], TorchLike): + v = torch.hstack([x[i].array for i in range(len(x))]) + else: + v = torch.FloatTensor(x) return TorchLike(v) @staticmethod diff --git a/tests/parametric/test_CasADi_computations_parametric.py b/tests/parametric/test_CasADi_computations_parametric.py new file mode 100644 index 00000000..6969bd44 --- /dev/null +++ b/tests/parametric/test_CasADi_computations_parametric.py @@ -0,0 +1,215 @@ +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import logging +from os import link +import casadi as cs +import numpy as np +import pytest +import math +from adam.parametric.casadi import KinDynComputationsParametric +from adam.casadi import KinDynComputations + +from adam.geometry import utils +import tempfile +from git import Repo +from adam import Representations + +# Getting stickbot urdf file +temp_dir = tempfile.TemporaryDirectory() +git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git" +Repo.clone_from(git_url, temp_dir.name) +model_path = temp_dir.name + "/models/stickBot/model.urdf" + +## Ack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49 +robot_file_read = open(model_path, "r") +robot_urdf_string = robot_file_read.read() +robot_urdf_string = robot_urdf_string.replace("", "") +robot_file_write = open(model_path, "w") +robot_file_write.write(robot_urdf_string) + +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", +] + + +logging.basicConfig(level=logging.DEBUG) +logging.debug("Showing the robot tree.") + + +root_link = "root_link" +comp = KinDynComputations(model_path, joints_name_list, root_link) + +link_name_list = ["chest"] +comp_w_hardware = KinDynComputationsParametric( + model_path, joints_name_list, link_name_list, root_link +) + +original_density = [ + 628.0724496264945 +] # This is the original density value associated to the chest link, computed as mass/volume +original_length = np.ones(len(link_name_list)) + + +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 + +H_b = utils.H_from_Pos_RPY(xyz, rpy) +vb_ = base_vel +s_ = joints_val +s_dot_ = joints_dot_val + + +def test_mass_matrix(): + M = comp.mass_matrix_fun() + M_with_hardware = comp_w_hardware.mass_matrix_fun() + mass_test = cs.DM(M(H_b, s_)) + mass_test_hardware = cs.DM( + M_with_hardware(H_b, s_, original_length, original_density) + ) + assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(): + Jcm = comp.centroidal_momentum_matrix_fun() + Jcm_with_hardware = comp_w_hardware.centroidal_momentum_matrix_fun() + Jcm_test = cs.DM(Jcm(H_b, s_)) + Jcm_test_hardware = cs.DM( + Jcm_with_hardware(H_b, s_, original_length, original_density) + ) + assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(): + com_f = comp.CoM_position_fun() + com_with_hardware_f = comp_w_hardware.CoM_position_fun() + CoM_cs = cs.DM(com_f(H_b, s_)) + CoM_hardware = cs.DM( + com_with_hardware_f(H_b, s_, original_length, original_density) + ) + assert CoM_cs - CoM_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_total_mass(): + mass = comp.get_total_mass() + mass_hardware_fun = comp_w_hardware.get_total_mass() + mass_hardware = cs.DM(mass_hardware_fun(original_length, original_density)) + assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian(): + J_tot = comp.jacobian_fun("l_sole") + J_tot_with_hardware = comp_w_hardware.jacobian_fun("l_sole") + J_test = cs.DM(J_tot(H_b, s_)) + J_test_hardware = cs.DM( + J_tot_with_hardware(H_b, s_, original_length, original_density) + ) + assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(): + J_tot = comp.jacobian_fun("head") + J_test = cs.DM(J_tot(H_b, s_)) + J_tot_with_hardware = comp_w_hardware.jacobian_fun("head") + J_tot_with_hardware_test = cs.DM( + J_tot_with_hardware(H_b, s_, original_length, original_density) + ) + assert J_test - J_tot_with_hardware_test == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(): + J_dot = comp.jacobian_dot_fun("l_sole") + J_dot_hardware = comp_w_hardware.jacobian_dot_fun("l_sole") + J_dot_nu_test = cs.DM( + J_dot(H_b, joints_val, base_vel, joints_dot_val) + @ np.concatenate((base_vel, joints_dot_val)) + ) + J_dot_nu_test2 = cs.DM( + J_dot_hardware( + H_b, joints_val, base_vel, joints_dot_val, original_length, original_density + ) + @ np.concatenate((base_vel, joints_dot_val)) + ) + assert J_dot_nu_test - J_dot_nu_test2 == pytest.approx(0.0, abs=1e-5) + + +def test_fk(): + T = comp.forward_kinematics_fun("l_sole") + H_test = cs.DM(T(H_b, s_)) + T_with_hardware = comp_w_hardware.forward_kinematics_fun("l_sole") + H_with_hardware_test = cs.DM( + T_with_hardware(H_b, s_, original_length, original_density) + ) + assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(): + T = comp.forward_kinematics_fun("head") + H_test = cs.DM(T(H_b, s_)) + T_with_hardware = comp_w_hardware.forward_kinematics_fun("head") + H_with_hardware_test = cs.DM( + T_with_hardware(H_b, s_, original_length, original_density) + ) + assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(): + h = comp.bias_force_fun() + h_test = cs.DM(h(H_b, s_, vb_, s_dot_)) + + h_with_hardware = comp_w_hardware.bias_force_fun() + h_with_hardware_test = cs.DM( + h_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density) + ) + assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + C = comp.coriolis_term_fun() + C_test = cs.DM(C(H_b, s_, vb_, s_dot_)) + C_with_hardware = comp_w_hardware.coriolis_term_fun() + C_with_hardware_test = cs.DM( + C_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density) + ) + assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(): + G = comp.gravity_term_fun() + G_test = cs.DM(G(H_b, s_)) + G_with_hardware = comp_w_hardware.gravity_term_fun() + G_with_hardware_test = G_with_hardware(H_b, s_, original_length, original_density) + assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/parametric/test_Jax_computations_parametric.py b/tests/parametric/test_Jax_computations_parametric.py new file mode 100644 index 00000000..1de2ac69 --- /dev/null +++ b/tests/parametric/test_Jax_computations_parametric.py @@ -0,0 +1,203 @@ +# 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 +from os import link +import urdf_parser_py.urdf +import jax.numpy as jnp +from jax import config +import numpy as np +import pytest +import math +from adam.parametric.jax import KinDynComputationsParametric +from adam.jax import KinDynComputations + +from adam.geometry import utils +import tempfile +from git import Repo +from adam import Representations + +np.random.seed(42) +config.update("jax_enable_x64", True) + +# Getting stickbot urdf file +temp_dir = tempfile.TemporaryDirectory() +git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git" +Repo.clone_from(git_url, temp_dir.name) +model_path = temp_dir.name + "/models/stickBot/model.urdf" + +## Ack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49 +robot_file_read = open(model_path, "r") +robot_urdf_string = robot_file_read.read() +robot_urdf_string = robot_urdf_string.replace("", "") +robot_file_write = open(model_path, "w") +robot_file_write.write(robot_urdf_string) + +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 SX2DM(x): + return cs.DM(x) + + +logging.basicConfig(level=logging.DEBUG) +logging.debug("Showing the robot tree.") + + +root_link = "root_link" +comp = KinDynComputations(model_path, joints_name_list, root_link) + +link_name_list = ["chest"] +comp_w_hardware = KinDynComputationsParametric( + model_path, joints_name_list, link_name_list, root_link +) + +original_density = [ + 628.0724496264945 +] # This is the original density value associated to the chest link, computed as mass/volume +original_length = np.ones(len(link_name_list)) + +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 + +H_b = utils.H_from_Pos_RPY(xyz, rpy) +vb_ = base_vel +s_ = joints_val +s_dot_ = joints_dot_val + + +def test_mass_matrix(): + mass_test = comp.mass_matrix(H_b, s_) + mass_test_hardware = comp_w_hardware.mass_matrix( + H_b, s_, original_length, original_density + ) + assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(): + Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) + Jcm_test_hardware = comp_w_hardware.centroidal_momentum_matrix( + H_b, s_, original_length, original_density + ) + + assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(): + CoM_test = comp.CoM_position(H_b, s_) + CoM_hardware = comp_w_hardware.CoM_position( + H_b, s_, original_length, original_density + ) + + assert CoM_test - CoM_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_total_mass(): + mass = comp.get_total_mass() + mass_hardware = comp_w_hardware.get_total_mass(original_length, original_density) + assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian(): + J_test = comp.jacobian("l_sole", H_b, s_) + J_test_hardware = comp_w_hardware.jacobian( + "l_sole", H_b, s_, original_length, original_density + ) + assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(): + J_test = comp.jacobian("head", H_b, s_) + J_test_hardware = comp_w_hardware.jacobian( + "head", H_b, s_, original_length, original_density + ) + assert J_test - J_test_hardware == 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) + J_dot_hardware = comp_w_hardware.jacobian_dot( + "l_sole", + H_b, + joints_val, + base_vel, + joints_dot_val, + original_length, + original_density, + ) + assert J_dot - J_dot_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_fk(): + H_test = comp.forward_kinematics("l_sole", H_b, s_) + H_with_hardware_test = comp_w_hardware.forward_kinematics( + "l_sole", H_b, s_, original_length, original_density + ) + assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(): + H_test = comp.forward_kinematics("head", H_b, s_) + H_with_hardware_test = comp_w_hardware.forward_kinematics( + "head", H_b, s_, original_length, original_density + ) + assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(): + h_test = comp.bias_force(H_b, s_, vb_, s_dot_) + h_with_hardware_test = comp_w_hardware.bias_force( + H_b, s_, vb_, s_dot_, original_length, original_density + ) + assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + C_test = comp.coriolis_term(H_b, s_, vb_, s_dot_) + C_with_hardware_test = comp_w_hardware.coriolis_term( + H_b, s_, vb_, s_dot_, original_length, original_density + ) + assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(): + G_test = comp.gravity_term(H_b, s_) + G_with_hardware_test = comp_w_hardware.gravity_term( + H_b, s_, original_length, original_density + ) + assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/parametric/test_NumPy_computations_parametric.py b/tests/parametric/test_NumPy_computations_parametric.py new file mode 100644 index 00000000..db54c120 --- /dev/null +++ b/tests/parametric/test_NumPy_computations_parametric.py @@ -0,0 +1,202 @@ +# 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 +from os import link +import urdf_parser_py.urdf +import pytest +import math +import numpy as np +from adam.parametric.numpy import KinDynComputationsParametric +from adam.numpy import KinDynComputations + +from adam.geometry import utils +import tempfile +from git import Repo +from adam import Representations + +np.random.seed(42) + +# Getting stickbot urdf file +temp_dir = tempfile.TemporaryDirectory() +git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git" +Repo.clone_from(git_url, temp_dir.name) +model_path = temp_dir.name + "/models/stickBot/model.urdf" + +## Ack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49 +robot_file_read = open(model_path, "r") +robot_urdf_string = robot_file_read.read() +robot_urdf_string = robot_urdf_string.replace("", "") +robot_file_write = open(model_path, "w") +robot_file_write.write(robot_urdf_string) + +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 SX2DM(x): + return cs.DM(x) + + +logging.basicConfig(level=logging.DEBUG) +logging.debug("Showing the robot tree.") + + +root_link = "root_link" +comp = KinDynComputations(model_path, joints_name_list, root_link) + +link_name_list = ["chest"] +comp_w_hardware = KinDynComputationsParametric( + model_path, joints_name_list, link_name_list, root_link +) +# comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) +# comp_w_hardware.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) + +original_density = [ + 628.0724496264945 +] # This is the original density value associated to the chest link, computed as mass/volume +original_length = np.ones(len(link_name_list)) + +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 + +H_b = utils.H_from_Pos_RPY(xyz, rpy) +vb_ = base_vel +s_ = joints_val +s_dot_ = joints_dot_val + + +def test_mass_matrix(): + mass_test = comp.mass_matrix(H_b, s_) + mass_test_hardware = comp_w_hardware.mass_matrix( + H_b, s_, original_length, original_density + ) + + assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(): + Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) + Jcm_test_hardware = comp_w_hardware.centroidal_momentum_matrix( + H_b, s_, original_length, original_density + ) + + assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(): + CoM_test = comp.CoM_position(H_b, s_) + CoM_hardware = comp_w_hardware.CoM_position( + H_b, s_, original_length, original_density + ) + assert CoM_test - CoM_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_total_mass(): + mass = comp.get_total_mass() + mass_hardware = comp_w_hardware.get_total_mass(original_length, original_density) + assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian(): + J_test = comp.jacobian("l_sole", H_b, s_) + J_test_hardware = comp_w_hardware.jacobian( + "l_sole", H_b, s_, original_length, original_density + ) + assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(): + J_test = comp.jacobian("head", H_b, s_) + J_test_hardware = comp_w_hardware.jacobian( + "head", H_b, s_, original_length, original_density + ) + assert J_test - J_test_hardware == 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) + J_dot_w_hardware = comp_w_hardware.jacobian_dot( + "l_sole", + H_b, + joints_val, + base_vel, + joints_dot_val, + original_length, + original_density, + ) + assert J_dot - J_dot_w_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_fk(): + H_test = comp.forward_kinematics("l_sole", H_b, s_) + H_with_hardware_test = comp_w_hardware.forward_kinematics( + "l_sole", H_b, s_, original_length, original_density + ) + assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(): + H_test = comp.forward_kinematics("head", H_b, s_) + H_with_hardware_test = comp_w_hardware.forward_kinematics( + "head", H_b, s_, original_length, original_density + ) + assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(): + h_test = comp.bias_force(H_b, s_, vb_, s_dot_) + h_with_hardware_test = comp_w_hardware.bias_force( + H_b, s_, vb_, s_dot_, original_length, original_density + ) + assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + C_test = comp.coriolis_term(H_b, s_, vb_, s_dot_) + C_with_hardware_test = comp_w_hardware.coriolis_term( + H_b, s_, vb_, s_dot_, original_length, original_density + ) + assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(): + G_test = comp.gravity_term(H_b, s_) + G_with_hardware_test = comp_w_hardware.gravity_term( + H_b, s_, original_length, original_density + ) + assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/parametric/test_pytorch_computations_parametric.py b/tests/parametric/test_pytorch_computations_parametric.py new file mode 100644 index 00000000..f40a89ee --- /dev/null +++ b/tests/parametric/test_pytorch_computations_parametric.py @@ -0,0 +1,212 @@ +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import logging +from os import link +import urdf_parser_py.urdf +import pytest +import math +import torch +import numpy as np +from adam.parametric.pytorch import KinDynComputationsParametric +from adam.pytorch import KinDynComputations + +from adam.geometry import utils +import tempfile +from git import Repo +from adam import Representations + +np.random.seed(42) +torch.set_default_dtype(torch.float64) + +# Getting stickbot urdf file +temp_dir = tempfile.TemporaryDirectory() +git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git" +Repo.clone_from(git_url, temp_dir.name) +model_path = temp_dir.name + "/models/stickBot/model.urdf" + +## Ack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49 +robot_file_read = open(model_path, "r") +robot_urdf_string = robot_file_read.read() +robot_urdf_string = robot_urdf_string.replace("", "") +robot_file_write = open(model_path, "w") +robot_file_write.write(robot_urdf_string) + +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 SX2DM(x): + return cs.DM(x) + + +logging.basicConfig(level=logging.DEBUG) +logging.debug("Showing the robot tree.") + + +root_link = "root_link" +comp = KinDynComputations(model_path, joints_name_list, root_link) + +link_name_list = ["chest"] +comp_w_hardware = KinDynComputationsParametric( + model_path, joints_name_list, link_name_list, root_link +) +# comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) +# comp_w_hardware.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) + +original_density = [ + 628.0724496264945 +] # This is the original density value associated to the chest link, computed as mass/volume +original_length = np.ones(len(link_name_list)) + +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 + +H_b = torch.FloatTensor(utils.H_from_Pos_RPY(xyz, rpy)) +vb_ = torch.FloatTensor(base_vel) +s_ = torch.FloatTensor(joints_val) +s_dot_ = torch.FloatTensor(joints_dot_val) + + +def test_mass_matrix(): + mass_test = comp.mass_matrix(H_b, s_) + mass_test_hardware = np.array( + comp_w_hardware.mass_matrix(H_b, s_, original_length, original_density) + ) + assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(): + Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) + Jcm_test_hardware = np.array( + comp_w_hardware.centroidal_momentum_matrix( + H_b, s_, original_length, original_density + ) + ) + assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(): + CoM_test = comp.CoM_position(H_b, s_) + CoM_hardware = np.array( + comp_w_hardware.CoM_position(H_b, s_, original_length, original_density) + ) + assert CoM_test - CoM_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_total_mass(): + mass = comp.get_total_mass() + mass_hardware = comp_w_hardware.get_total_mass(original_length, original_density) + assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian(): + J_test = comp.jacobian("l_sole", H_b, s_) + J_test_hardware = np.array( + comp_w_hardware.jacobian("l_sole", H_b, s_, original_length, original_density) + ) + assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(): + J_test = comp.jacobian("head", H_b, s_) + J_test_hardware = np.array( + comp_w_hardware.jacobian("head", H_b, s_, original_length, original_density) + ) + assert J_test - J_test_hardware == 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) + J_dot_hardware = comp_w_hardware.jacobian_dot( + "l_sole", + H_b, + joints_val, + base_vel, + joints_dot_val, + original_length, + original_density, + ) + assert J_dot - J_dot_hardware == pytest.approx(0.0, abs=1e-5) + + +def test_fk(): + H_test = np.array(comp.forward_kinematics("l_sole", H_b, s_)) + H_with_hardware_test = np.array( + comp_w_hardware.forward_kinematics( + "l_sole", H_b, s_, original_length, original_density + ) + ) + assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(): + H_test = np.array(comp.forward_kinematics("head", H_b, s_)) + H_with_hardware_test = np.array( + comp_w_hardware.forward_kinematics( + "head", H_b, s_, original_length, original_density + ) + ) + assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(): + h_test = np.array(comp.bias_force(H_b, s_, vb_, s_dot_)) + h_with_hardware_test = np.array( + comp_w_hardware.bias_force( + H_b, s_, vb_, s_dot_, original_length, original_density + ) + ) + assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + C_test = np.array(comp.coriolis_term(H_b, s_, vb_, s_dot_)) + C_with_hardware_test = np.array( + comp_w_hardware.coriolis_term( + H_b, s_, vb_, s_dot_, original_length, original_density + ) + ) + assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(): + G_test = comp.gravity_term(H_b, s_) + G_with_hardware_test = comp_w_hardware.gravity_term( + H_b, s_, original_length, original_density + ) + assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4)