diff --git a/iCubGazeboV2_5.xml b/iCubGazeboV2_5.xml new file mode 100644 index 00000000..9060a608 --- /dev/null +++ b/iCubGazeboV2_5.xml @@ -0,0 +1,349 @@ + + + + + + + + + + + + + + + diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index 8b0dc66d..734b1982 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -2,14 +2,16 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +from pathlib import Path +from typing import Union + import casadi as cs import numpy as np -from typing import Union from adam.casadi.casadi_like import SpatialMath from adam.core import RBDAlgorithms from adam.core.constants import Representations -from adam.model import Model, URDFModelFactory +from adam.model import MJModelFactory, Model, utils class KinDynComputations: @@ -19,7 +21,7 @@ class KinDynComputations: def __init__( self, - urdfstring: str, + model_string: Union[str, Path], joints_name_list: list = None, root_link: str = "root_link", gravity: np.array = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]), @@ -27,18 +29,62 @@ def __init__( ) -> None: """ Args: - urdfstring (str): either path or string of the urdf + model_string (Union[str, Path]): path or string of the URDF model, or the Mujoco XML file joints_name_list (list): list of the actuated joints root_link (str, optional): the first link. Defaults to 'root_link'. """ math = SpatialMath() - factory = URDFModelFactory(path=urdfstring, math=math) + factory = utils.get_factory_from_string(model_string=model_string, math=math) 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 + @staticmethod + def from_urdf( + urdf_string: Union[str, Path], + joints_name_list: list = None, + gravity: np.array = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]), + ) -> "KinDynComputations": + """Creates a KinDynComputations object from a URDF string + + Args: + urdf_string (Union[str, Path]): The URDF string or path + joints_name_list (list, optional): List of the actuated joints. Defaults to None. + gravity (np.array, optional): The gravity vector. Defaults to np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]). + + Returns: + KinDynComputations: The KinDynComputations object + """ + return KinDynComputations( + model_string=urdf_string, + joints_name_list=joints_name_list, + gravity=gravity, + ) + + @staticmethod + def from_mujoco_xml( + xml_string: Union[str, Path], + joints_name_list: list = None, + gravity: np.array = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]), + ) -> "KinDynComputations": + """Creates a KinDynComputations object from a Mujoco XML string + + Args: + xml_string (Union[str, Path]): The Mujoco XML path + joints_name_list (list, optional): List of the actuated joints. Defaults to None. + gravity (np.array, optional): The gravity vector. Defaults to np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]). + + Returns: + KinDynComputations: The KinDynComputations object + """ + return KinDynComputations( + model_string=xml_string, + joints_name_list=joints_name_list, + gravity=gravity, + ) + def set_frame_velocity_representation( self, representation: Representations ) -> None: diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index ba68391f..d740d2b2 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -16,9 +16,8 @@ class RBDAlgorithms: def __init__(self, model: Model, math: SpatialMath) -> None: """ Args: - urdfstring (str): path of the urdf - joints_name_list (list): list of the actuated joints - root_link (str, optional): the first link. Defaults to 'root_link'. + model (Model): The robot model + math (SpatialMath): The spatial math object """ self.model = model diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index 25ad6adc..53a029ce 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -2,13 +2,16 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +from pathlib import Path +from typing import Union + import jax.numpy as jnp import numpy as np from adam.core.constants import Representations from adam.core.rbd_algorithms import RBDAlgorithms from adam.jax.jax_like import SpatialMath -from adam.model import Model, URDFModelFactory +from adam.model import Model, utils class KinDynComputations: @@ -16,24 +19,66 @@ class KinDynComputations: def __init__( self, - urdfstring: str, + model_string: str, joints_name_list: list = None, root_link: str = "root_link", gravity: np.array = jnp.array([0, 0, -9.80665, 0, 0, 0]), ) -> None: """ Args: - urdfstring (str): either path or string of the urdf + model_string (Union[str, Path]): the path or string of the URDF, or MuJoCo XML file joints_name_list (list): list of the actuated joints root_link (str, optional): the first link. Defaults to 'root_link'. """ math = SpatialMath() - factory = URDFModelFactory(path=urdfstring, math=math) + factory = utils.get_factory_from_string(model_string, math) 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 + @staticmethod + def from_urdf( + urdf_string: Union[str, Path], + joints_name_list: list = None, + gravity: np.array = jnp.array([0, 0, -9.80665, 0, 0, 0]), + ): + """Creates a KinDynComputations object from a URDF string + + Args: + urdf_string (Union[str, Path]): The URDF path or string + joints_name_list (list, optional): The actuated joints. Defaults to None. + root_link (str, optional): The root link. Defaults to "root_link". + gravity (np.array, optional): The gravity vector. Defaults to jnp.array([0, 0, -9.80665, 0, 0, 0]). + + Returns: + KinDynComputations: The KinDynComputations object + """ + return KinDynComputations( + model_string=urdf_string, joints_name_list=joints_name_list, gravity=gravity + ) + + @staticmethod + def from_mujoco_xml( + xml_string: Union[str, Path], + joints_name_list: list = None, + gravity: np.array = jnp.array([0, 0, -9.80665, 0, 0, 0]), + ): + """Creates a KinDynComputations object from a MuJoCo XML string + + Args: + xml_string (Union[str, Path]): The MuJoCo XML path + joints_name_list (list, optional): The actuated joints. Defaults to None. + root_link (str, optional): The root link. Defaults to "root_link". + gravity (np.array, optional): The gravity vector. Defaults to jnp.array([0, 0, -9.80665, 0, 0, 0]). + + Returns: + KinDynComputations: The KinDynComputations object + """ + return KinDynComputations( + model_string=xml_string, joints_name_list=joints_name_list, gravity=gravity + ) + def set_frame_velocity_representation( self, representation: Representations ) -> None: diff --git a/src/adam/model/__init__.py b/src/adam/model/__init__.py index 0426e317..f61b610f 100644 --- a/src/adam/model/__init__.py +++ b/src/adam/model/__init__.py @@ -1,4 +1,7 @@ -from .abc_factories import Joint, Link, ModelFactory, Inertial, Pose +from .abc_factories import Inertia, Inertial, Joint, Link, ModelFactory, Pose +from .mj_factories.mj_joint import MJJoint +from .mj_factories.mj_link import MJLink +from .mj_factories.mj_model import MJModelFactory 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 4720588f..372b228f 100644 --- a/src/adam/model/abc_factories.py +++ b/src/adam/model/abc_factories.py @@ -54,33 +54,84 @@ class Joint(abc.ABC): Abstract base class for all joints. """ - @abc.abstractmethod - def spatial_transform(self, q: npt.ArrayLike) -> npt.ArrayLike: + def homogeneous(self, q: npt.ArrayLike) -> npt.ArrayLike: """ Args: - q (npt.ArrayLike): joint motion + q (npt.ArrayLike): joint value Returns: - npt.ArrayLike: spatial transform of the joint given q + npt.ArrayLike: the homogenous transform of a joint, given q """ - pass - @abc.abstractmethod - def motion_subspace(self) -> npt.ArrayLike: + if self.type == "fixed": + xyz = self.origin.xyz + rpy = self.origin.rpy + return self.math.H_from_Pos_RPY(xyz, rpy) + elif self.type in ["revolute", "continuous"]: + return self.math.H_revolute_joint( + self.origin.xyz, + self.origin.rpy, + self.axis, + q, + ) + elif self.type in ["prismatic"]: + return self.math.H_prismatic_joint( + self.origin.xyz, + self.origin.rpy, + self.axis, + q, + ) + + def spatial_transform(self, q: npt.ArrayLike) -> npt.ArrayLike: """ + Args: + q (npt.ArrayLike): joint motion + Returns: - npt.ArrayLike: motion subspace of the joint + npt.ArrayLike: spatial transform of the joint given q """ + if self.type == "fixed": + return self.math.X_fixed_joint(self.origin.xyz, self.origin.rpy) + elif self.type in ["revolute", "continuous"]: + return self.math.X_revolute_joint( + self.origin.xyz, self.origin.rpy, self.axis, q + ) + elif self.type in ["prismatic"]: + return self.math.X_prismatic_joint( + self.origin.xyz, + self.origin.rpy, + self.axis, + q, + ) - @abc.abstractmethod - def homogeneous(self, q: npt.ArrayLike) -> npt.ArrayLike: + def motion_subspace(self) -> npt.ArrayLike: """ Args: - q (npt.ArrayLike): joint value + joint (Joint): Joint + Returns: - npt.ArrayLike: homogeneous transform given the joint value + npt.ArrayLike: motion subspace of the joint """ - pass + 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, + ) @dataclasses.dataclass @@ -130,22 +181,27 @@ class Link(abc.ABC): inertial: Inertial collisions: List - @abc.abstractmethod def spatial_inertia(self) -> npt.ArrayLike: """ Returns: npt.ArrayLike: the 6x6 inertia matrix expressed at the origin of the link (with rotation) """ - pass + I = self.inertial.inertia + mass = self.inertial.mass + o = self.inertial.origin.xyz + rpy = self.inertial.origin.rpy + return self.math.spatial_inertia(I, mass, o, rpy) - @abc.abstractmethod def homogeneous(self) -> npt.ArrayLike: """ Returns: npt.ArrayLike: the homogeneous transform of the link """ - pass + return self.math.H_from_Pos_RPY( + self.inertial.origin.xyz, + self.inertial.origin.rpy, + ) @dataclasses.dataclass diff --git a/src/adam/model/mj_factories/__init__.py b/src/adam/model/mj_factories/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/adam/model/mj_factories/mj_joint.py b/src/adam/model/mj_factories/mj_joint.py new file mode 100644 index 00000000..93933f3c --- /dev/null +++ b/src/adam/model/mj_factories/mj_joint.py @@ -0,0 +1,36 @@ +from typing import Dict, List, Optional + +from scipy.spatial.transform import Rotation + +from adam.core.spatial_math import SpatialMath +from adam.model import Joint, Pose + + +def convert_data(data: str) -> List[float]: + """Convert strings to floats.""" + return list(map(float, data.split())) + + +def wxyz_to_xyzw(quat: List[float]) -> List[float]: + """Convert quaternion from Mujoco (wxyz) to scipy (xyzw) convention.""" + return [quat[1], quat[2], quat[3], quat[0]] + + +class MJJoint(Joint): + """MuJoCo Joint class.""" + + def __init__( + self, joint: Dict, math: SpatialMath, idx: Optional[int] = None + ) -> None: + self.math = math + self.name = joint["name"] + self.parent = joint["parent"] + self.child = joint["child"] + self.type = joint["joint_type"] + self.axis = convert_data(joint["axis"]) + joint_ori_xyz = convert_data(joint["origin"]) + joint_quat = wxyz_to_xyzw(convert_data(joint["orientation"])) + joint_rpy = Rotation.from_quat(joint_quat).as_euler("xyz") + self.origin = Pose(xyz=joint_ori_xyz, rpy=joint_rpy) + self.limit = joint["limit"] + self.idx = idx diff --git a/src/adam/model/mj_factories/mj_link.py b/src/adam/model/mj_factories/mj_link.py new file mode 100644 index 00000000..b6f6c7bb --- /dev/null +++ b/src/adam/model/mj_factories/mj_link.py @@ -0,0 +1,65 @@ +from typing import Dict, List + +import numpy as np +import numpy.typing as npt +from scipy.spatial.transform import Rotation as R + +from adam.core.spatial_math import SpatialMath +from adam.model import Inertia, Inertial, Link, Pose + + +def convert_data(data: str) -> List[float]: + """Convert strings to floats.""" + return list(map(float, data.split())) + + +def wxyz_to_xyzw(quat: List[float]) -> List[float]: + """Convert quaternion from Mujoco (wxyz) to scipy (xyzw) convention.""" + return [quat[1], quat[2], quat[3], quat[0]] + + +class MJLink(Link): + """MuJoCo Link class.""" + + def __init__(self, link: Dict, math: SpatialMath): + self.math = math + self.name = link["name"] + self.visuals = link["visuals"] + self.collisions = link["collisions"] + + self.inertial = ( + self.from_mjcf_inertia(link["inertial"]) + if link["inertial"] + else Inertial.zero() + ) + + def from_mjcf_inertia(self, inertial: Dict) -> Inertial: + """ + Args: + inertial (Dict): The inertial properties of the link. + + Returns: + Inertial: The inertial properties of the link. + """ + diaginertia = convert_data(inertial["diaginertia"]) + quat = wxyz_to_xyzw(convert_data(inertial["quat"])) + pos = convert_data(inertial["pos"]) + mass = float(inertial["mass"]) + + rotation_matrix = R.from_quat(quat).as_matrix() + I = self.transform_inertia_tensor(rotation_matrix, diaginertia) + + inertia = Inertia( + ixx=I[0, 0], iyy=I[1, 1], izz=I[2, 2], ixy=I[0, 1], ixz=I[0, 2], iyz=I[1, 2] + ) + origin = Pose(xyz=pos, rpy=[0.0, 0.0, 0.0]) + + return Inertial(mass=mass, inertia=inertia, origin=origin) + + @staticmethod + def transform_inertia_tensor( + rotation_matrix: np.ndarray, diaginertia: List[float] + ) -> np.ndarray: + """Transform the diagonal inertia tensor to the original frame.""" + I_diag = np.diag(diaginertia) + return rotation_matrix @ I_diag @ rotation_matrix.T diff --git a/src/adam/model/mj_factories/mj_model.py b/src/adam/model/mj_factories/mj_model.py new file mode 100644 index 00000000..791585e2 --- /dev/null +++ b/src/adam/model/mj_factories/mj_model.py @@ -0,0 +1,175 @@ +import os +import pathlib +import xml.etree.ElementTree as ET +from typing import List + +from adam.core.spatial_math import SpatialMath +from adam.model import MJJoint, MJLink, ModelFactory + + +def parse_mjcf(file_path): + tree = ET.parse(file_path) + return tree.getroot() + + +def build_tree(root): + """Build the tree structure of the robot from the MJCF file. + + Args: + root: the root of the xml tree + + Returns: + Tuple: the links, joints, tree_structure, child_map, parent_map + """ + + tree_structure = {} + links = {} + joints = {} + child_map = {} + parent_map = {} + + def add_body(body, parent=None): + body_name = body.get("name", "unnamed") + visuals = [visual.attrib for visual in body.findall("geom")] + inertial = body.find("inertial") + inertial_info = inertial.attrib if inertial is not None else None + collisions = [collision.attrib for collision in body.findall("collision")] + + link_info = { + "name": body_name, + "visuals": visuals, + "inertial": inertial_info, + "collisions": collisions, + "pos": body.get("pos", "0 0 0"), + "quat": body.get("quat", "1 0 0 0"), + } + links[body_name] = link_info + if parent: + joint = body.find("joint") + joint_info = { + "name": ( + joint.get("name", f"{parent}_{body_name}_joint") + if joint is not None + else f"{parent}_{body_name}_joint" + ), + "parent": parent, + "child": body_name, + "joint_type": ( + joint.get("type", "revolute") if joint is not None else "fixed" + ), + "axis": joint.get("axis", "0 0 1") if joint is not None else "0 0 1", + "origin": link_info["pos"], + "limit": joint.get("range", "0 0") if joint is not None else "0 0", + "orientation": link_info["quat"], + } + # raise error if joint type is not supported + if joint_info["joint_type"] not in ["revolute", "prismatic", "fixed"]: + raise ValueError(f"Joint type {joint_info['joint_type']} not supported") + + joints[joint_info["name"]] = joint_info + tree_structure[body_name] = (parent, joint_info["name"]) + child_map[parent] = body_name + parent_map[body_name] = parent + + for child in body.findall("body"): + add_body(child, body_name) + + worldbody = root.find("worldbody") + if worldbody is not None: + for body in worldbody.findall("body"): + add_body(body) + else: + raise ValueError("No worldbody found") + + return links, joints, tree_structure, child_map, parent_map + + +class MJModelFactory(ModelFactory): + """This factory generates robot elements from a MuJoCo XML file. + + Args: + ModelFactory: the Model factory + """ + + def __init__(self, path: str, math: SpatialMath): + self.math = math + xml_root = parse_mjcf(path) + ( + self.links, + self.joints, + self.tree_structure, + self.child_map, + self.parent_map, + ) = build_tree(xml_root) + self.name = xml_root.get("model", "model") + + def get_joints(self) -> List[MJJoint]: + """ + Returns: + List[MJJoint]: build the list of the joints + """ + return [self.build_joint(j) for j in self.joints.values()] + + def get_links(self) -> List[MJLink]: + """ + Returns: + List[MJLink]: 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.links.values() + if l["inertial"] is not None + or l["name"] in self.child_map.keys() + or any( + j["joint_type"] != "fixed" + for j in self.joints.values() + if j["child"] == l["name"] + ) + ] + + def get_frames(self) -> List[MJLink]: + """ + Returns: + List[MJLink]: 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.links.values() + if l["inertial"] is None + and l["name"] not in self.child_map.keys() + and all( + j["joint_type"] == "fixed" + for j in self.joints.values() + if j["child"] == l["name"] + ) + ] + + def build_joint(self, joint) -> MJJoint: + """ + Args: + joint (Joint): the joint + + Returns: + MJJoint: our joint representation + """ + return MJJoint(joint, self.math) + + def build_link(self, link) -> MJLink: + """ + Args: + link (Link): the link + + Returns: + MJLink: our link representation + """ + return MJLink(link, self.math) diff --git a/src/adam/model/std_factories/std_joint.py b/src/adam/model/std_factories/std_joint.py index a72787d8..5bec788a 100644 --- a/src/adam/model/std_factories/std_joint.py +++ b/src/adam/model/std_factories/std_joint.py @@ -1,6 +1,5 @@ from typing import Union -import numpy.typing as npt import urdf_parser_py.urdf from adam.core.spatial_math import SpatialMath @@ -25,82 +24,3 @@ def __init__( self.origin = joint.origin self.limit = joint.limit self.idx = idx - - 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 - """ - - if self.type == "fixed": - xyz = self.origin.xyz - rpy = self.origin.rpy - return self.math.H_from_Pos_RPY(xyz, rpy) - elif self.type in ["revolute", "continuous"]: - return self.math.H_revolute_joint( - self.origin.xyz, - self.origin.rpy, - self.axis, - q, - ) - elif self.type in ["prismatic"]: - return self.math.H_prismatic_joint( - self.origin.xyz, - self.origin.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.xyz, self.origin.rpy) - elif self.type in ["revolute", "continuous"]: - return self.math.X_revolute_joint( - self.origin.xyz, self.origin.rpy, self.axis, q - ) - elif self.type in ["prismatic"]: - return self.math.X_prismatic_joint( - self.origin.xyz, - self.origin.rpy, - 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/model/std_factories/std_link.py b/src/adam/model/std_factories/std_link.py index 7754747d..e4af5857 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, Inertial, Pose +from adam.model import Inertial, Link, Pose class StdLink(Link): @@ -24,25 +24,3 @@ def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath): link.inertial.origin = Pose(xyz=[0, 0, 0], rpy=[0, 0, 0]) self.inertial = link.inertial - - def spatial_inertia(self) -> npt.ArrayLike: - """ - Returns: - npt.ArrayLike: the 6x6 inertia matrix expressed at - the origin of the link (with rotation) - """ - I = self.inertial.inertia - mass = self.inertial.mass - o = self.inertial.origin.xyz - rpy = self.inertial.origin.rpy - return self.math.spatial_inertia(I, mass, o, rpy) - - def homogeneous(self) -> npt.ArrayLike: - """ - Returns: - npt.ArrayLike: the homogeneous transform of the link - """ - return self.math.H_from_Pos_RPY( - self.inertial.origin.xyz, - self.inertial.origin.rpy, - ) diff --git a/src/adam/model/std_factories/std_model.py b/src/adam/model/std_factories/std_model.py index 3649a0b1..f52e906e 100644 --- a/src/adam/model/std_factories/std_model.py +++ b/src/adam/model/std_factories/std_model.py @@ -1,7 +1,8 @@ +import os import pathlib -from typing import List import xml.etree.ElementTree as ET -import os +from typing import List + import urdf_parser_py.urdf from adam.core.spatial_math import SpatialMath @@ -17,10 +18,7 @@ def urdf_remove_sensors_tags(xml_string): for sensors_tag in root.findall("sensor"): root.remove(sensors_tag) - # Convert the modified XML back to a string - modified_xml_string = ET.tostring(root) - - return modified_xml_string + return ET.tostring(root) def get_xml_string(path: str): diff --git a/src/adam/model/tree.py b/src/adam/model/tree.py index 73d36210..f78e02bf 100644 --- a/src/adam/model/tree.py +++ b/src/adam/model/tree.py @@ -71,15 +71,15 @@ def build_tree(links: List[Link], joints: List[Joint]) -> "Tree": raise ValueError("The model has more than one root link") return Tree(nodes, root_link[0]) - def print(self, root): - """prints the tree + def show(self): + """prints the tree""" + self.print() - Args: - root (str): the root of the tree - """ + def print(self): + """prints the tree""" import pptree - pptree.print_tree(self.graph[root]) + pptree.print_tree(self.graph[self.root]) def get_ordered_nodes_list(self, start: str) -> List[str]: """get the ordered list of the nodes, given the connectivity diff --git a/src/adam/model/utils.py b/src/adam/model/utils.py new file mode 100644 index 00000000..bce81afe --- /dev/null +++ b/src/adam/model/utils.py @@ -0,0 +1,37 @@ +from pathlib import Path +from typing import Union + +from adam.core.spatial_math import SpatialMath +from adam.model import MJModelFactory, ModelFactory, URDFModelFactory + + +def get_factory_from_string( + model_string: str, math: SpatialMath +) -> Union[MJModelFactory, URDFModelFactory]: + """Get the factory from the model string (path or string) and the math library + + Args: + model_string (str): the path or the string of the model + math (SpatialMath): the math library + + Returns: + ModelFactory: the factory that generates the model + """ + + if Path(model_string).exists(): + model_string = Path(model_string) + if model_string.suffix == ".xml": + print("Loading the model from MJCF file") + return MJModelFactory(path=model_string, math=math) + elif model_string.suffix == ".urdf": + print("Loading the model from URDF file") + return URDFModelFactory(path=model_string, math=math) + else: + raise ValueError( + f"The file {model_string} is not a valid MJCF or URDF file" + ) + elif isinstance(model_string, str): + print("Loading the model from string in URDF format") + return URDFModelFactory(urdf_string=model_string, math=math) + else: + raise ValueError("The model_string is not a valid path or string") diff --git a/src/adam/numpy/computations.py b/src/adam/numpy/computations.py index 0b72019a..02e39b6e 100644 --- a/src/adam/numpy/computations.py +++ b/src/adam/numpy/computations.py @@ -2,11 +2,14 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +from pathlib import Path +from typing import Union + import numpy as np from adam.core.constants import Representations from adam.core.rbd_algorithms import RBDAlgorithms -from adam.model import Model, URDFModelFactory +from adam.model import Model, utils from adam.numpy.numpy_like import SpatialMath @@ -15,24 +18,68 @@ class KinDynComputations: def __init__( self, - urdfstring: str, + model_string: Union[str, Path], joints_name_list: list = None, root_link: str = "root_link", gravity: np.array = np.array([0, 0, -9.80665, 0, 0, 0]), ) -> None: """ Args: - urdfstring (str): either path or string of the urdf + model_string (Union[str, Path]): path or string of the URDF model, or the Mujoco XML file joints_name_list (list): list of the actuated joints root_link (str, optional): the first link. Defaults to 'root_link'. """ math = SpatialMath() - factory = URDFModelFactory(path=urdfstring, math=math) + factory = utils.get_factory_from_string(model_string=model_string, math=math) model = Model.build(factory=factory, joints_name_list=joints_name_list) self.rbdalgos = RBDAlgorithms(model=model, math=math) self.NDoF = model.NDoF self.g = gravity + @staticmethod + def from_urdf( + urdf_string: Union[str, Path], + joints_name_list: list = None, + gravity: np.array = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]), + ) -> "KinDynComputations": + """Creates a KinDynComputations object from a URDF string + + Args: + urdf_string (Union[str, Path]): The URDF path or string + joints_name_list (list, optional): List of the actuated joints. Defaults to None. + gravity (np.array, optional): The gravity vector. Defaults to np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]). + + Returns: + KinDynComputations: The KinDynComputations object + """ + return KinDynComputations( + model_string=urdf_string, + joints_name_list=joints_name_list, + gravity=gravity, + ) + + @staticmethod + def from_mujoco_xml( + xml_string: Union[str, Path], + joints_name_list: list = None, + gravity: np.array = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]), + ) -> "KinDynComputations": + """Creates a KinDynComputations object from a Mujoco XML string + + Args: + xml_string (Union[str, Path]): The Mujoco XML path + joints_name_list (list, optional): List of the actuated joints. Defaults to None. + gravity (np.array, optional): The gravity vector. Defaults to np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]). + + Returns: + KinDynComputations: The KinDynComputations object + """ + return KinDynComputations( + model_string=xml_string, + joints_name_list=joints_name_list, + gravity=gravity, + ) + def set_frame_velocity_representation( self, representation: Representations ) -> None: diff --git a/src/adam/pytorch/computation_batch.py b/src/adam/pytorch/computation_batch.py index b5e96a10..1e4e2adb 100644 --- a/src/adam/pytorch/computation_batch.py +++ b/src/adam/pytorch/computation_batch.py @@ -2,6 +2,9 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +from pathlib import Path +from typing import Union + import jax import jax.numpy as jnp import numpy as np @@ -11,7 +14,7 @@ from adam.core.constants import Representations from adam.core.rbd_algorithms import RBDAlgorithms from adam.jax.jax_like import SpatialMath -from adam.model import Model, URDFModelFactory +from adam.model import Model, utils class KinDynComputationsBatch: @@ -21,25 +24,65 @@ class KinDynComputationsBatch: def __init__( self, - urdfstring: str, + model_string: Union[str, Path], joints_name_list: list = None, root_link: str = "root_link", - gravity: np.array = jnp.array([0, 0, -9.80665, 0, 0, 0]), + gravity: jnp.array = jnp.array([0, 0, -9.80665, 0, 0, 0]), ) -> None: """ Args: - urdfstring (str): path of the urdf + model_string (Union[str, Path]): the path or string of the URDF, or Mujoco XML file joints_name_list (list): list of the actuated joints root_link (str, optional): the first link. Defaults to 'root_link'. """ math = SpatialMath() - factory = URDFModelFactory(path=urdfstring, math=math) + factory = utils.get_factory_from_string(model_string=model_string, math=math) 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.funcs = {} + @staticmethod + def from_urdf( + urdf_string: Union[str, Path], + joints_name_list: list = None, + gravity: jnp.array = jnp.array([0, 0, -9.80665, 0, 0, 0]), + ): + """Initializes the class using a URDF string + + Args: + urdf_string (Union[str, Path]): The URDF path or string + joints_name_list (list, optional): The list of actuated joints. Defaults to None. + gravity (np.array, optional): The gravity vector. Defaults to jnp.array([0, 0, -9.80665, 0, 0, 0]). + + Returns: + KinDynComputationsBatch: The class instance + """ + return KinDynComputationsBatch( + model_string=urdf_string, joints_name_list=joints_name_list, gravity=gravity + ) + + @staticmethod + def from_mujoco_xml( + xml_string: Union[str, Path], + joints_name_list: list = None, + gravity: jnp.array = jnp.array([0, 0, -9.80665, 0, 0, 0]), + ): + """Initializes the class using a Mujoco XML string + + Args: + xml_string (Union[str, Path]): The Mujoco XML path + joints_name_list (list, optional): The list of actuated joints. Defaults to None. + gravity (np.array, optional): The gravity vector. Defaults to jnp.array([0, 0, -9.80665, 0, 0, 0]). + + Returns: + KinDynComputationsBatch: The class instance + """ + return KinDynComputationsBatch( + model_string=xml_string, joints_name_list=joints_name_list, gravity=gravity + ) + def set_frame_velocity_representation( self, representation: Representations ) -> None: diff --git a/src/adam/pytorch/computations.py b/src/adam/pytorch/computations.py index 060a2075..8b2aa440 100644 --- a/src/adam/pytorch/computations.py +++ b/src/adam/pytorch/computations.py @@ -2,12 +2,15 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +from pathlib import Path +from typing import Union + import numpy as np import torch from adam.core.constants import Representations from adam.core.rbd_algorithms import RBDAlgorithms -from adam.model import Model, URDFModelFactory +from adam.model import Model, utils from adam.pytorch.torch_like import SpatialMath @@ -16,26 +19,70 @@ class KinDynComputations: def __init__( self, - urdfstring: str, + model_string: Union[str, Path], joints_name_list: list = None, root_link: str = "root_link", - gravity: np.array = torch.tensor( + gravity: torch.tensor = torch.tensor( [0, 0, -9.80665, 0, 0, 0], dtype=torch.float64 ), ) -> None: """ Args: - urdfstring (str): either path or string of the urdf + model_string (Union[str, Path]): path or string of the URDF model, or the Mujoco XML file joints_name_list (list): list of the actuated joints root_link (str, optional): the first link. Defaults to 'root_link'. """ math = SpatialMath() - factory = URDFModelFactory(path=urdfstring, math=math) + factory = utils.get_factory_from_string(model_string=model_string, math=math) 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 + @staticmethod + def from_urdf( + urdf_string: Union[str, Path], + joints_name_list: list = None, + gravity: torch.tensor = torch.tensor( + [0, 0, -9.80665, 0, 0, 0], dtype=torch.float64 + ), + ) -> "KinDynComputations": + """Creates a KinDynComputations object from a URDF string + + Args: + urdf_string (Union[str, Path]): The URDF string or path + joints_name_list (list): list of the actuated joints + gravity (torch.tensor, optional): The gravity vector. Defaults to torch.tensor([0, 0, -9.80665, 0, 0, 0], dtype=torch.float64). + + Returns: + KinDynComputations: The KinDynComputations object + """ + return KinDynComputations( + model_string=urdf_string, joints_name_list=joints_name_list, gravity=gravity + ) + + @staticmethod + def from_mujoco_xml( + xml_string: Union[str, Path], + joints_name_list: list = None, + gravity: torch.tensor = torch.tensor( + [0, 0, -9.80665, 0, 0, 0], dtype=torch.float64 + ), + ) -> "KinDynComputations": + """Creates a KinDynComputations object from a Mujoco XML string + + Args: + xml_string (Union[str, Path]): The Mujoco XML path + joints_name_list (list): list of the actuated joints + gravity (torch.tensor, optional): The gravity vector. Defaults to torch.tensor([0, 0, -9.80665, 0, 0, 0], dtype=torch.float64). + + Returns: + KinDynComputations: The KinDynComputations object + """ + return KinDynComputations( + model_string=xml_string, joints_name_list=joints_name_list, gravity=gravity + ) + def set_frame_velocity_representation( self, representation: Representations ) -> None: diff --git a/tests/from_mujoco/test_CasADi_mj.py b/tests/from_mujoco/test_CasADi_mj.py new file mode 100644 index 00000000..216d2792 --- /dev/null +++ b/tests/from_mujoco/test_CasADi_mj.py @@ -0,0 +1,240 @@ +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import logging + +import casadi as cs +import icub_models +import idyntree.swig as idyntree +import numpy as np +import pytest + +from adam import Representations +from adam.casadi import KinDynComputations +from adam.geometry import utils + +np.random.seed(42) + +model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) +model_path_xml = "iCubGazeboV2_5.xml" + +joints_name_list = [ + "torso_pitch", + "torso_roll", + "torso_yaw", + "l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "l_hip_pitch", + "l_hip_roll", + "l_hip_yaw", + "l_knee", + "l_ankle_pitch", + "l_ankle_roll", + "r_hip_pitch", + "r_hip_roll", + "r_hip_yaw", + "r_knee", + "r_ankle_pitch", + "r_ankle_roll", +] + + +def H_from_Pos_RPY_idyn(xyz, rpy): + T = idyntree.Transform.Identity() + R = idyntree.Rotation.RPY(rpy[0], rpy[1], rpy[2]) + p = idyntree.Position() + [p.setVal(i, xyz[i]) for i in range(3)] + T.setRotation(R) + T.setPosition(p) + return T + + +logging.basicConfig(level=logging.DEBUG) +logging.debug("Showing the robot tree.") + +comp = KinDynComputations.from_mujoco_xml(model_path_xml, joints_name_list) +comp.set_frame_velocity_representation(Representations.MIXED_REPRESENTATION) +robot_iDyn = idyntree.ModelLoader() +robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) + +kinDyn = idyntree.KinDynComputations() +kinDyn.loadRobotModel(robot_iDyn.model()) +kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) +n_dofs = len(joints_name_list) + +# base pose quantities +xyz = (np.random.rand(3) - 0.5) * 5 +rpy = (np.random.rand(3) - 0.5) * 5 +base_vel = (np.random.rand(6) - 0.5) * 5 +# joints quantitites +joints_val = (np.random.rand(n_dofs) - 0.5) * 5 +joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 + +g = np.array([0, 0, -9.80665]) +H_b = utils.H_from_Pos_RPY(xyz, rpy) +kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) + + +def test_mass_matrix(): + M = comp.mass_matrix_fun() + mass_mx = idyntree.MatrixDynSize() + kinDyn.getFreeFloatingMassMatrix(mass_mx) + mass_mxNumpy = mass_mx.toNumPy() + mass_test = cs.DM(M(H_b, joints_val)) + mass_test2 = cs.DM(comp.mass_matrix(H_b, joints_val)) + assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) + assert mass_test2 - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(): + Jcm = comp.centroidal_momentum_matrix_fun() + cmm_idyntree = idyntree.MatrixDynSize() + kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) + cmm_idyntreeNumpy = cmm_idyntree.toNumPy() + Jcm_test = cs.DM(Jcm(H_b, joints_val)) + Jcm_test2 = cs.DM(comp.centroidal_momentum_matrix(H_b, joints_val)) + assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) + assert Jcm_test2 - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(): + com_f = comp.CoM_position_fun() + CoM_test = cs.DM(com_f(H_b, joints_val)) + CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() + CoM_test2 = cs.DM(comp.CoM_position(H_b, joints_val)) + assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) + assert CoM_test2 - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) + + +def test_total_mass(): + assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx( + 0.0, abs=1e-5 + ) + + +def test_jacobian(): + J_tot = comp.jacobian_fun("l_ankle_2") + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("l_ankle_2", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_test = cs.DM(J_tot(H_b, joints_val)) + J_test2 = cs.DM(comp.jacobian("l_ankle_2", H_b, joints_val)) + assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) + assert iDynNumpyJ_ - J_test2 == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(): + J_tot = comp.jacobian_fun("head") + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_test = cs.DM(J_tot(H_b, joints_val)) + J_test2 = cs.DM(comp.jacobian("head", H_b, joints_val)) + assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) + assert iDynNumpyJ_ - J_test2 == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(): + J_dot = comp.jacobian_dot_fun("l_ankle_2") + Jdotnu = kinDyn.getFrameBiasAcc("l_ankle_2") + Jdot_nu = Jdotnu.toNumPy() + J_dot_nu_test = J_dot(H_b, joints_val, base_vel, joints_dot_val) @ np.concatenate( + (base_vel, joints_dot_val) + ) + J_dot_nu_test2 = cs.DM( + comp.jacobian_dot("l_ankle_2", H_b, joints_val, base_vel, joints_dot_val) + ) @ np.concatenate((base_vel, joints_dot_val)) + assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) + assert Jdot_nu - J_dot_nu_test2 == pytest.approx(0.0, abs=1e-5) + + +def test_fk(): + H_idyntree = kinDyn.getWorldTransform("l_ankle_2") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + T = comp.forward_kinematics_fun("l_ankle_2") + H_test = cs.DM(T(H_b, joints_val)) + H_test2 = cs.DM(comp.forward_kinematics("l_ankle_2", H_b, joints_val)) + assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + assert R_idy2np - H_test2[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert p_idy2np - H_test2[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(): + H_idyntree = kinDyn.getWorldTransform("head") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + T = comp.forward_kinematics_fun("head") + H_test = cs.DM(T(H_b, joints_val)) + H_test2 = cs.DM(comp.forward_kinematics("head", H_b, joints_val)) + assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + assert R_idy2np - H_test2[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert p_idy2np - H_test2[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(): + h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(h_iDyn) + h_iDyn_np = np.concatenate( + (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) + ) + h = comp.bias_force_fun() + h_test = cs.DM(h(H_b, joints_val, base_vel, joints_dot_val)) + h_test2 = cs.DM(comp.bias_force(H_b, joints_val, base_vel, joints_dot_val)) + assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) + assert h_iDyn_np - h_test2 == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + g0 = idyntree.Vector3() + g0.zero() + kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) + C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(C_iDyn) + C_iDyn_np = np.concatenate( + (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) + ) + C = comp.coriolis_term_fun() + C_test = cs.DM(C(H_b, joints_val, base_vel, joints_dot_val)) + C_test2 = cs.DM(comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val)) + assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) + assert C_iDyn_np - C_test2 == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(): + base_vel_0 = np.zeros(6) + joints_dot_val_0 = np.zeros(n_dofs) + kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) + kinDyn.setRobotState(H_b, joints_val, base_vel_0, joints_dot_val_0, g) + G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(G_iDyn) + G_iDyn_np = np.concatenate( + (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) + ) + G = comp.gravity_term_fun() + G_test = cs.DM(G(H_b, joints_val)) + G_test2 = cs.DM(comp.gravity_term(H_b, joints_val)) + assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) + assert G_iDyn_np - G_test2 == pytest.approx(0.0, abs=1e-4) + + +def test_relative_jacobian(): + eye = np.eye(4) + kinDyn.setRobotState(eye, joints_val, base_vel, joints_dot_val, g) + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("l_ankle_2", iDyntreeJ_) + iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] + J_fun = comp.relative_jacobian_fun("l_ankle_2") + J_test = cs.DM(J_fun(joints_val)) + J_test2 = cs.DM(comp.relative_jacobian("l_ankle_2", joints_val)) + assert iDynNumpyRelativeJ - J_test == pytest.approx(0.0, abs=1e-4) + assert iDynNumpyRelativeJ - J_test2 == pytest.approx(0.0, abs=1e-4) diff --git a/tests/from_mujoco/test_Jax_mj.py b/tests/from_mujoco/test_Jax_mj.py new file mode 100644 index 00000000..9b799485 --- /dev/null +++ b/tests/from_mujoco/test_Jax_mj.py @@ -0,0 +1,193 @@ +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import logging + +import icub_models +import idyntree.swig as idyntree +import jax.numpy as jnp +import numpy as np +import pytest +from jax import config + +import adam +from adam.geometry import utils +from adam.jax import KinDynComputations + +np.random.seed(42) +config.update("jax_enable_x64", True) + +model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) +model_path_xml = "iCubGazeboV2_5.xml" + +joints_name_list = [ + "torso_pitch", + "torso_roll", + "torso_yaw", + "l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "l_hip_pitch", + "l_hip_roll", + "l_hip_yaw", + "l_knee", + "l_ankle_pitch", + "l_ankle_roll", + "r_hip_pitch", + "r_hip_roll", + "r_hip_yaw", + "r_knee", + "r_ankle_pitch", + "r_ankle_roll", +] + + +def H_from_Pos_RPY_idyn(xyz, rpy): + T = idyntree.Transform.Identity() + R = idyntree.Rotation.RPY(rpy[0], rpy[1], rpy[2]) + p = idyntree.Position() + [p.setVal(i, xyz[i]) for i in range(3)] + T.setRotation(R) + T.setPosition(p) + return T + + +logging.basicConfig(level=logging.DEBUG) +logging.debug("Showing the robot tree.") + +comp = KinDynComputations.from_mujoco_xml(model_path_xml, joints_name_list) + +comp.set_frame_velocity_representation(adam.Representations.MIXED_REPRESENTATION) +robot_iDyn = idyntree.ModelLoader() +robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) + +kinDyn = idyntree.KinDynComputations() +kinDyn.loadRobotModel(robot_iDyn.model()) +kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) +n_dofs = len(joints_name_list) + +# base pose quantities +xyz = (np.random.rand(3) - 0.5) * 5 +rpy = (np.random.rand(3) - 0.5) * 5 +base_vel = (np.random.rand(6) - 0.5) * 5 +# joints quantitites +joints_val = (np.random.rand(n_dofs) - 0.5) * 5 +joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 + +g = np.array([0, 0, -9.80665]) +H_b = utils.H_from_Pos_RPY(xyz, rpy) +kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) + + +def test_mass_matrix(): + mass_mx = idyntree.MatrixDynSize() + kinDyn.getFreeFloatingMassMatrix(mass_mx) + mass_mxNumpy = mass_mx.toNumPy() + mass_test = comp.mass_matrix(H_b, joints_val) + assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(): + cmm_idyntree = idyntree.MatrixDynSize() + kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) + cmm_idyntreeNumpy = cmm_idyntree.toNumPy() + Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) + assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(): + CoM_test = comp.CoM_position(H_b, joints_val) + CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() + assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) + + +def test_total_mass(): + assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx( + 0.0, abs=1e-5 + ) + + +def test_jacobian(): + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("l_hand", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_test = comp.jacobian("l_hand", H_b, joints_val) + assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(): + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_test = comp.jacobian("head", H_b, joints_val) + assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(): + J_dot = comp.jacobian_dot("l_hand", H_b, joints_val, base_vel, joints_dot_val) + Jdotnu = kinDyn.getFrameBiasAcc("l_hand") + Jdot_nu = Jdotnu.toNumPy() + J_dot_nu_test = J_dot @ np.concatenate((base_vel, joints_dot_val)) + assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) + + +def test_fk(): + H_idyntree = kinDyn.getWorldTransform("l_hand") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + H_test = comp.forward_kinematics("l_hand", H_b, joints_val) + assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(): + H_idyntree = kinDyn.getWorldTransform("head") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + H_test = comp.forward_kinematics("head", H_b, joints_val) + assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(): + h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(h_iDyn) + h_iDyn_np = np.concatenate( + (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) + ) + h_test = comp.bias_force(H_b, joints_val, base_vel, joints_dot_val) + assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + g0 = np.zeros(3) + kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) + C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(C_iDyn) + C_iDyn_np = np.concatenate( + (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) + ) + C_test = comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val) + assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(): + kinDyn2 = idyntree.KinDynComputations() + kinDyn2.loadRobotModel(robot_iDyn.model()) + kinDyn2.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) + base_vel0 = np.zeros(6) + joints_dot_val0 = np.zeros(n_dofs) + kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) + G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) + assert kinDyn2.generalizedBiasForces(G_iDyn) + G_iDyn_np = np.concatenate( + (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) + ) + G_test = comp.gravity_term(H_b, joints_val) + assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/from_mujoco/test_NumPy_mj.py b/tests/from_mujoco/test_NumPy_mj.py new file mode 100644 index 00000000..0a2f201a --- /dev/null +++ b/tests/from_mujoco/test_NumPy_mj.py @@ -0,0 +1,191 @@ +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import logging + +import icub_models +import idyntree.swig as idyntree +import numpy as np +import pytest + +from adam import Representations +from adam.geometry import utils +from adam.numpy import KinDynComputations + +np.random.seed(42) + +model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) +model_path_xml = "iCubGazeboV2_5.xml" + +joints_name_list = [ + "torso_pitch", + "torso_roll", + "torso_yaw", + "l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "l_hip_pitch", + "l_hip_roll", + "l_hip_yaw", + "l_knee", + "l_ankle_pitch", + "l_ankle_roll", + "r_hip_pitch", + "r_hip_roll", + "r_hip_yaw", + "r_knee", + "r_ankle_pitch", + "r_ankle_roll", +] + + +def H_from_Pos_RPY_idyn(xyz, rpy): + T = idyntree.Transform.Identity() + R = idyntree.Rotation.RPY(rpy[0], rpy[1], rpy[2]) + p = idyntree.Position() + [p.setVal(i, xyz[i]) for i in range(3)] + T.setRotation(R) + T.setPosition(p) + return T + + +logging.basicConfig(level=logging.DEBUG) +logging.debug("Showing the robot tree.") + +comp = KinDynComputations.from_mujoco_xml( + xml_string=model_path_xml, joints_name_list=joints_name_list +) +robot_iDyn = idyntree.ModelLoader() +robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) +# robot_iDyn.loadModelFromFile(model_path) + +kinDyn = idyntree.KinDynComputations() +kinDyn.loadRobotModel(robot_iDyn.model()) +kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) +n_dofs = comp.NDoF + +# base pose quantities +xyz = (np.random.rand(3) - 0.5) * 5 +rpy = (np.random.rand(3) - 0.5) * 5 +base_vel = (np.random.rand(6) - 0.5) * 5 +# joints quantitites +joints_val = (np.random.rand(n_dofs) - 0.5) * 5 +joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 + +g = np.array([0, 0, -9.80665]) +H_b = utils.H_from_Pos_RPY(xyz, rpy) +kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) + + +def test_mass_matrix(): + mass_mx = idyntree.MatrixDynSize() + kinDyn.getFreeFloatingMassMatrix(mass_mx) + mass_mxNumpy = mass_mx.toNumPy() + mass_test = comp.mass_matrix(H_b, joints_val) + assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(): + cmm_idyntree = idyntree.MatrixDynSize() + kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) + cmm_idyntreeNumpy = cmm_idyntree.toNumPy() + Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) + assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(): + CoM_test = comp.CoM_position(H_b, joints_val) + CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() + assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) + + +def test_total_mass(): + assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx( + 0.0, abs=1e-5 + ) + + +def test_jacobian(): + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("l_ankle_2", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_test = comp.jacobian("l_ankle_2", H_b, joints_val) + assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(): + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_test = comp.jacobian("head", H_b, joints_val) + assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(): + J_dot = comp.jacobian_dot("l_ankle_2", H_b, joints_val, base_vel, joints_dot_val) + Jdotnu = kinDyn.getFrameBiasAcc("l_ankle_2") + Jdot_nu = Jdotnu.toNumPy() + J_dot_nu_test = J_dot @ np.concatenate((base_vel, joints_dot_val)) + assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) + + +def test_fk(): + H_idyntree = kinDyn.getWorldTransform("l_hand") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + H_test = comp.forward_kinematics("l_hand", H_b, joints_val) + assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(): + H_idyntree = kinDyn.getWorldTransform("head") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + H_test = comp.forward_kinematics("head", H_b, joints_val) + assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(): + h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(h_iDyn) + h_iDyn_np = np.concatenate( + (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) + ) + h_test = comp.bias_force(H_b, joints_val, base_vel, joints_dot_val) + assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + g0 = np.zeros(3) + kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) + C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(C_iDyn) + C_iDyn_np = np.concatenate( + (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) + ) + C_test = comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val) + assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(): + kinDyn2 = idyntree.KinDynComputations() + kinDyn2.loadRobotModel(robot_iDyn.model()) + kinDyn2.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) + base_vel0 = np.zeros(6) + joints_dot_val0 = np.zeros(n_dofs) + kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) + G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) + assert kinDyn2.generalizedBiasForces(G_iDyn) + G_iDyn_np = np.concatenate( + (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) + ) + G_test = comp.gravity_term(H_b, joints_val) + assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/from_mujoco/test_pytorch_mj.py b/tests/from_mujoco/test_pytorch_mj.py new file mode 100644 index 00000000..dfd23e09 --- /dev/null +++ b/tests/from_mujoco/test_pytorch_mj.py @@ -0,0 +1,205 @@ +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +import logging + +import icub_models +import idyntree.swig as idyntree +import numpy as np +import pytest +import torch + +from adam import Representations +from adam.geometry import utils +from adam.pytorch import KinDynComputations +from adam.pytorch.torch_like import SpatialMath + +np.random.seed(42) +torch.set_default_dtype(torch.float64) + +model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) +model_path_xml = "iCubGazeboV2_5.xml" + +joints_name_list = [ + "torso_pitch", + "torso_roll", + "torso_yaw", + "l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "l_hip_pitch", + "l_hip_roll", + "l_hip_yaw", + "l_knee", + "l_ankle_pitch", + "l_ankle_roll", + "r_hip_pitch", + "r_hip_roll", + "r_hip_yaw", + "r_knee", + "r_ankle_pitch", + "r_ankle_roll", +] + + +def H_from_Pos_RPY_idyn(xyz, rpy): + T = idyntree.Transform.Identity() + R = idyntree.Rotation.RPY(rpy[0], rpy[1], rpy[2]) + p = idyntree.Position() + [p.setVal(i, xyz[i]) for i in range(3)] + T.setRotation(R) + T.setPosition(p) + return T + + +logging.basicConfig(level=logging.DEBUG) +logging.debug("Showing the robot tree.") + +comp = KinDynComputations.from_mujoco_xml(model_path_xml, joints_name_list) +comp.set_frame_velocity_representation(Representations.MIXED_REPRESENTATION) +robot_iDyn = idyntree.ModelLoader() +robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) + +kinDyn = idyntree.KinDynComputations() +kinDyn.loadRobotModel(robot_iDyn.model()) +kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) +n_dofs = len(joints_name_list) + +xyz = (torch.rand(3) - 0.5) * 5 +rpy = (torch.rand(3) - 0.5) * 5 +base_vel = (torch.rand(6) - 0.5) * 5 +# joints quantitites +joints_val = (torch.rand(n_dofs) - 0.5) * 5 +joints_dot_val = (torch.rand(n_dofs) - 0.5) * 5 + +g = torch.tensor([0, 0, -9.80665]) +H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array +kinDyn.setRobotState( + H_b.numpy(), joints_val.numpy(), base_vel.numpy(), joints_dot_val.numpy(), g.numpy() +) + + +def test_mass_matrix(): + mass_mx = idyntree.MatrixDynSize() + kinDyn.getFreeFloatingMassMatrix(mass_mx) + mass_mxNumpy = mass_mx.toNumPy() + mass_test = comp.mass_matrix(H_b, joints_val) + assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(): + cmm_idyntree = idyntree.MatrixDynSize() + kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) + cmm_idyntreeNumpy = cmm_idyntree.toNumPy() + Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) + assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(): + CoM_test = comp.CoM_position(H_b, joints_val) + CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() + assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) + + +def test_total_mass(): + assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx( + 0.0, abs=1e-5 + ) + + +def test_jacobian(): + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("l_ankle_2", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_test = comp.jacobian("l_ankle_2", H_b, joints_val) + assert iDynNumpyJ_ - np.asarray(J_test) == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(): + iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) + kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) + iDynNumpyJ_ = iDyntreeJ_.toNumPy() + J_test = comp.jacobian("head", H_b, joints_val) + assert iDynNumpyJ_ - np.asarray(J_test) == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(): + J_dot = comp.jacobian_dot("l_ankle_2", H_b, joints_val, base_vel, joints_dot_val) + Jdotnu = kinDyn.getFrameBiasAcc("l_ankle_2") + Jdot_nu = Jdotnu.toNumPy() + J_dot_nu_test = J_dot @ np.concatenate((base_vel, joints_dot_val)) + assert Jdot_nu - np.asarray(J_dot_nu_test) == pytest.approx(0.0, abs=1e-5) + + +def test_fk(): + H_idyntree = kinDyn.getWorldTransform("l_ankle_2") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + H_test = np.asarray(comp.forward_kinematics("l_ankle_2", H_b, joints_val)) + assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(): + H_idyntree = kinDyn.getWorldTransform("head") + p_idy2np = H_idyntree.getPosition().toNumPy() + R_idy2np = H_idyntree.getRotation().toNumPy() + H_test = np.asarray(comp.forward_kinematics("head", H_b, joints_val)) + assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) + assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(): + h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(h_iDyn) + h_iDyn_np = np.concatenate( + (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) + ) + h_test = comp.bias_force(H_b, joints_val, base_vel, joints_dot_val) + assert h_iDyn_np - np.asarray(h_test) == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(): + g0 = torch.zeros(3) + kinDyn.setRobotState( + H_b.numpy(), + joints_val.numpy(), + base_vel.numpy(), + joints_dot_val.numpy(), + g0.numpy(), + ) + C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) + assert kinDyn.generalizedBiasForces(C_iDyn) + C_iDyn_np = np.concatenate( + (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) + ) + C_test = comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val) + assert C_iDyn_np - np.asarray(C_test) == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(): + kinDyn2 = idyntree.KinDynComputations() + kinDyn2.loadRobotModel(robot_iDyn.model()) + kinDyn2.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) + base_vel0 = torch.zeros(6) + joints_dot_val0 = torch.zeros(n_dofs) + kinDyn2.setRobotState( + H_b.numpy(), + joints_val.numpy(), + base_vel0.numpy(), + joints_dot_val0.numpy(), + g.numpy(), + ) + G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) + assert kinDyn2.generalizedBiasForces(G_iDyn) + G_iDyn_np = np.concatenate( + (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) + ) + G_test = comp.gravity_term(H_b, joints_val) + assert G_iDyn_np - np.asarray(G_test) == pytest.approx(0.0, abs=1e-4)