RBD and iDynTree high-level wrappers

python/gym_ignition/rbd/__init__.py

@@ -0,0 +1,6 @@
+# Copyright (C) 2020 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 . import idyntree
+from . import conversions

python/gym_ignition/rbd/conversions.py

@@ -0,0 +1,87 @@
+import abc
+import numpy as np
+from typing import Tuple
+from scipy.spatial.transform import Rotation
+
+
+class Transform(abc.ABC):
+
+    @staticmethod
+    def from_position_and_quaternion(position: np.ndarray,
+                                     quaternion: np.ndarray) -> np.ndarray:
+
+        if quaternion.size != 4:
+            raise ValueError("Quaternion array must have 4 elements")
+
+        rotation = Quaternion.to_rotation(quaternion)
+        transform = Transform.from_position_and_rotation(position=position,
+                                                         rotation=rotation)
+
+        return transform
+
+    @staticmethod
+    def from_position_and_rotation(position: np.ndarray,
+                                   rotation: np.ndarray) -> np.ndarray:
+
+        if position.size != 3:
+            raise ValueError("Position array must have 3 elements")
+
+        if rotation.shape != (3, 3):
+            raise ValueError("Rotation must be a square 3x3 matrix")
+
+        transform = np.eye(4)
+        transform[0:3, 3] = position
+        transform[0:3, 0:3] = rotation
+
+        return transform
+
+    @staticmethod
+    def get_position_and_rotation(transform: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
+
+        if transform.shape != (4, 4):
+            raise ValueError("Transform must be a 4x4 matrix")
+
+        position = transform[0:3, 3]
+        rotation = transform[0:3, 0:3]
+
+        return position, rotation
+
+
+class Quaternion(abc.ABC):
+
+    @staticmethod
+    def to_wxyz(xyzw: np.ndarray) -> np.ndarray:
+
+        if xyzw.shape != (4,):
+            raise ValueError(xyzw)
+
+        return xyzw[[3, 0, 1, 2]]
+
+    @staticmethod
+    def to_xyzw(wxyz: np.ndarray) -> np.ndarray:
+
+        if wxyz.shape != (4,):
+            raise ValueError(wxyz)
+
+        return wxyz[[1, 2, 3, 0]]
+
+    @staticmethod
+    def to_rotation(quaternion: np.ndarray) -> np.ndarray:
+
+        if quaternion.shape != (4,):
+            raise ValueError(quaternion)
+
+        xyzw = Quaternion.to_xyzw(quaternion)
+
+        return Rotation.from_quat(xyzw).as_matrix()
+
+    @staticmethod
+    def from_matrix(matrix: np.ndarray) -> np.ndarray:
+
+        if matrix.shape != (3, 3):
+            raise ValueError(matrix)
+
+        quaternion_xyzw = Rotation.from_matrix(matrix).as_quat()
+        quaternion_wxyz = Quaternion.to_wxyz(quaternion_xyzw)
+
+        return quaternion_wxyz

python/gym_ignition/rbd/idyntree/__init__.py

@@ -0,0 +1,7 @@
+# Copyright (C) 2020 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 . import numpy
+from . import helpers
+from . import kindyncomputations

python/gym_ignition/rbd/idyntree/helpers.py

@@ -0,0 +1,86 @@
+# Copyright (C) 2020 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 os
+import abc
+from typing import List
+from enum import Enum, auto
+import idyntree.bindings as idt
+from gym_ignition.utils import resource_finder
+
+
+class FrameVelocityRepresentation(Enum):
+
+    MIXED_REPRESENTATION = auto()
+    BODY_FIXED_REPRESENTATION = auto()
+    INERTIAL_FIXED_REPRESENTATION = auto()
+
+    def to_idyntree(self):
+
+        if self.value == FrameVelocityRepresentation.MIXED_REPRESENTATION.value:
+            return idt.MIXED_REPRESENTATION
+        elif self.value == FrameVelocityRepresentation.BODY_FIXED_REPRESENTATION.value:
+            return idt.BODY_FIXED_REPRESENTATION
+        elif self.value == FrameVelocityRepresentation.INERTIAL_FIXED_REPRESENTATION.value:
+            return idt.INERTIAL_FIXED_REPRESENTATION
+        else:
+            raise ValueError(self.value)
+
+
+class iDynTreeHelpers(abc.ABC):
+
+    @staticmethod
+    def get_model_loader(model_file: str, considered_joints: List[str] = None):
+
+        # Find the urdf file
+        urdf_file = resource_finder.find_resource(file_name=model_file)
+
+        # Get the file extension
+        folder, model_file = os.path.split(urdf_file)
+        model_name, extension = os.path.splitext(model_file)
+
+        if extension == ".sdf":
+            raise RuntimeError("SDF models are not currently supported by iDynTree")
+
+        # Create the model loader
+        mdl_loader = idt.ModelLoader()
+
+        # Load the urdf model
+        if considered_joints is None:
+            ok_load = mdl_loader.loadModelFromFile(urdf_file)
+        else:
+            ok_load = mdl_loader.loadReducedModelFromFile(urdf_file, considered_joints)
+
+        if not ok_load:
+            raise RuntimeError(f"Failed to load model from file '{urdf_file}'")
+
+        return mdl_loader
+
+    @staticmethod
+    def get_kindyncomputations(
+            model_file: str,
+            considered_joints: List[str] = None,
+            velocity_representation: FrameVelocityRepresentation = None):
+
+        # Get the model loader
+        model_loader = iDynTreeHelpers.get_model_loader(model_file, considered_joints)
+
+        # Create KinDynComputations and insert the model
+        kindyn = idt.KinDynComputations()
+        ok_load = kindyn.loadRobotModel(model_loader.model())
+
+        if not ok_load:
+            raise RuntimeError("Failed to load model")
+
+        if velocity_representation is None:
+            velocity_representation = FrameVelocityRepresentation.MIXED_REPRESENTATION
+
+        # Configure the velocity representation
+        velocity_representation_idyntree = velocity_representation.to_idyntree()
+        ok_repr = kindyn.setFrameVelocityRepresentation(velocity_representation_idyntree)
+
+        if not ok_repr:
+            raise RuntimeError("Failed to set the velocity representation")
+
+        return kindyn