diff --git a/examples/ik.ipynb b/examples/ik.ipynb index ab0444f8..27f5de72 100644 --- a/examples/ik.ipynb +++ b/examples/ik.ipynb @@ -7,7 +7,7 @@ "colab_type": "text" }, "source": [ - "\"Open" + "\"Open" ] }, { @@ -306,4 +306,4 @@ }, "nbformat": 4, "nbformat_minor": 0 -} \ No newline at end of file +} diff --git a/examples/mpc-ik.ipynb b/examples/mpc-ik.ipynb index 25c6a16e..ba527fcc 100644 --- a/examples/mpc-ik.ipynb +++ b/examples/mpc-ik.ipynb @@ -7,7 +7,7 @@ "colab_type": "text" }, "source": [ - "\"Open" + "\"Open" ] }, { @@ -338,4 +338,4 @@ }, "nbformat": 4, "nbformat_minor": 0 -} \ No newline at end of file +} diff --git a/src/adam/casadi/casadi_like.py b/src/adam/casadi/casadi_like.py index 9ee0ce02..1a9ae888 100644 --- a/src/adam/casadi/casadi_like.py +++ b/src/adam/casadi/casadi_like.py @@ -103,16 +103,18 @@ def T(self) -> "CasadiLike": class CasadiLikeFactory(ArrayLikeFactory): - @staticmethod - def zeros(*x: int) -> "CasadiLike": + + def __init__(self, cs_type: Union[cs.SX, cs.DM]): + self.cs_type = cs_type + + def zeros(self, *x: int) -> "CasadiLike": """ Returns: CasadiLike: Matrix of zeros of dim *x """ - return CasadiLike(cs.SX.zeros(*x)) + return CasadiLike(self.cs_type.zeros(*x)) - @staticmethod - def eye(x: int) -> "CasadiLike": + def eye(self, x: int) -> "CasadiLike": """ Args: x (int): matrix dimension @@ -120,20 +122,20 @@ def eye(x: int) -> "CasadiLike": Returns: CasadiLike: Identity matrix """ - return CasadiLike(cs.SX.eye(x)) + return CasadiLike(self.cs_type.eye(x)) - @staticmethod - def array(*x) -> "CasadiLike": + def array(self, *x) -> "CasadiLike": """ Returns: CasadiLike: Vector wrapping *x """ - return CasadiLike(cs.SX(*x)) + return CasadiLike(self.cs_type(*x)) class SpatialMath(SpatialMath): - def __init__(self): - super().__init__(CasadiLikeFactory) + + def __init__(self, cs_type: Union[cs.SX, cs.DM]): + super().__init__(CasadiLikeFactory(cs_type)) @staticmethod def skew(x: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike": diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index 58ff04a1..2612f4c1 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -22,6 +22,7 @@ def __init__( urdfstring: str, joints_name_list: list = None, root_link: str = "root_link", + cs_type: Union[cs.SX, cs.DM] = cs.SX, 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: @@ -31,7 +32,7 @@ def __init__( joints_name_list (list): list of the actuated joints root_link (str, optional): the first link. Defaults to 'root_link'. """ - math = SpatialMath() + math = SpatialMath(cs_type) factory = URDFModelFactory(path=urdfstring, math=math) model = Model.build(factory=factory, joints_name_list=joints_name_list) self.rbdalgos = RBDAlgorithms(model=model, math=math) diff --git a/src/adam/model/__init__.py b/src/adam/model/__init__.py index 0dd4bf9e..0426e317 100644 --- a/src/adam/model/__init__.py +++ b/src/adam/model/__init__.py @@ -1,4 +1,4 @@ -from .abc_factories import Joint, Link, ModelFactory +from .abc_factories import Joint, Link, ModelFactory, Inertial, Pose from .model import Model from .std_factories.std_joint import StdJoint from .std_factories.std_link import StdLink diff --git a/src/adam/model/abc_factories.py b/src/adam/model/abc_factories.py index 853dbb8a..4720588f 100644 --- a/src/adam/model/abc_factories.py +++ b/src/adam/model/abc_factories.py @@ -88,8 +88,36 @@ class Inertial: """Inertial description""" mass: npt.ArrayLike - inertia = Inertia - origin = Pose + inertia: Inertia + origin: Pose + + @staticmethod + def zero() -> "Inertial": + """Returns an Inertial object with zero mass and inertia""" + return Inertial( + mass=0.0, + inertia=Inertia( + ixx=0.0, + ixy=0.0, + ixz=0.0, + iyy=0.0, + iyz=0.0, + izz=0.0, + ), + origin=Pose(xyz=[0.0, 0.0, 0.0], rpy=[0.0, 0.0, 0.0]), + ) + + def set_mass(self, mass: npt.ArrayLike) -> "Inertial": + """Set the mass of the inertial object""" + self.mass = mass + + def set_inertia(self, inertia: Inertia) -> "Inertial": + """Set the inertia of the inertial object""" + self.inertia = inertia + + def set_origin(self, origin: Pose) -> "Inertial": + """Set the origin of the inertial object""" + self.origin = origin @dataclasses.dataclass diff --git a/src/adam/model/std_factories/std_link.py b/src/adam/model/std_factories/std_link.py index 9d90829d..7754747d 100644 --- a/src/adam/model/std_factories/std_link.py +++ b/src/adam/model/std_factories/std_link.py @@ -2,7 +2,7 @@ import urdf_parser_py.urdf from adam.core.spatial_math import SpatialMath -from adam.model import Link +from adam.model import Link, Inertial, Pose class StdLink(Link): @@ -15,10 +15,15 @@ def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath): self.inertial = link.inertial self.collisions = link.collisions + # if the link has no inertial properties (a connecting frame), let's add them + if link.inertial is None: + link.inertial = Inertial.zero() + # if the link has inertial properties, but the origin is None, let's add it if link.inertial is not None and link.inertial.origin is None: - link.inertial.origin.xyz = [0, 0, 0] - link.inertial.origin.rpy = [0, 0, 0] + link.inertial.origin = Pose(xyz=[0, 0, 0], rpy=[0, 0, 0]) + + self.inertial = link.inertial def spatial_inertia(self) -> npt.ArrayLike: """ diff --git a/src/adam/model/std_factories/std_model.py b/src/adam/model/std_factories/std_model.py index ed379af7..ded6bff5 100644 --- a/src/adam/model/std_factories/std_model.py +++ b/src/adam/model/std_factories/std_model.py @@ -69,6 +69,8 @@ def __init__(self, path: str, math: SpatialMath): # to have a useless and noisy warning, let's remove before hands all the sensor elements, # that anyhow are not parser by urdf_parser_py or adam # See https://github.com/ami-iit/ADAM/issues/59 + with open(path, "r") as xml_file: + xml_string = xml_file.read() xml_string_without_sensors_tags = urdf_remove_sensors_tags(xml_string) self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_string( xml_string_without_sensors_tags @@ -86,17 +88,45 @@ def get_links(self) -> List[StdLink]: """ Returns: List[StdLink]: build the list of the links + + A link is considered a "real" link if + - it has an inertial + - it has children + - if it has no children and no inertial, it is at lest connected to the parent with a non fixed joint """ return [ - self.build_link(l) for l in self.urdf_desc.links if l.inertial is not None + self.build_link(l) + for l in self.urdf_desc.links + if ( + l.inertial is not None + or l.name in self.urdf_desc.child_map.keys() + or any( + j.type != "fixed" + for j in self.urdf_desc.joints + if j.child == l.name + ) + ) ] def get_frames(self) -> List[StdLink]: """ Returns: List[StdLink]: build the list of the links + + A link is considered a "fake" link (frame) if + - it has no inertial + - it does not have children + - it is connected to the parent with a fixed joint """ - return [self.build_link(l) for l in self.urdf_desc.links if l.inertial is None] + return [ + self.build_link(l) + for l in self.urdf_desc.links + if l.inertial is None + and l.name not in self.urdf_desc.child_map.keys() + and all( + j.type == "fixed" for j in self.urdf_desc.joints if j.child == l.name + ) + ] def build_joint(self, joint: urdf_parser_py.urdf.Joint) -> StdJoint: """ diff --git a/src/adam/parametric/casadi/computations_parametric.py b/src/adam/parametric/casadi/computations_parametric.py index a6f44021..6be83ff2 100644 --- a/src/adam/parametric/casadi/computations_parametric.py +++ b/src/adam/parametric/casadi/computations_parametric.py @@ -1,7 +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 typing import List +from typing import List, Union import casadi as cs import numpy as np @@ -24,6 +24,7 @@ def __init__( joints_name_list: list, links_name_list: list, root_link: str = "root_link", + cs_type: Union[cs.SX, cs.DM] = cs.SX, 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: @@ -34,7 +35,7 @@ def __init__( links_name_list (list): list of the parametrized links root_link (str, optional): the first link. Defaults to 'root_link'. """ - math = SpatialMath() + math = SpatialMath(cs_type) 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) diff --git a/src/adam/parametric/model/parametric_factories/parametric_link.py b/src/adam/parametric/model/parametric_factories/parametric_link.py index c9ae02be..1346f3f8 100644 --- a/src/adam/parametric/model/parametric_factories/parametric_link.py +++ b/src/adam/parametric/model/parametric_factories/parametric_link.py @@ -50,10 +50,11 @@ def __init__( length_multiplier=self.length_multiplier ) self.mass = self.compute_mass() - self.inertial = Inertial(self.mass) - self.inertial.mass = self.mass - self.inertial.inertia = self.compute_inertia_parametric() - self.inertial.origin = self.modify_origin() + inertia_parametric = self.compute_inertia_parametric() + origin = self.modify_origin() + self.inertial = Inertial( + mass=self.mass, inertia=inertia_parametric, origin=origin + ) self.update_visuals() def get_principal_length(self):