update parsers/generators and simulator

pyrobolearn/simulators/mujoco.py

@@ -537,6 +537,8 @@ class Mujoco(Simulator):
         Returns:
             int (non-negative): unique id associated to the load model.
         """
+        print(filename)
+        print(os.path.dirname(filename))
         # parse URDF file
         urdf_parser = URDFParser(filename=filename)
         tree = urdf_parser.tree
@@ -546,7 +548,7 @@ class Mujoco(Simulator):
         tree.orientation = orientation
 
         # add the parse tree to the MJCF parser/generator
-        self._parser.add_multibody(tree)
+        self._parser.add_multibody(tree, mesh_directory_path=os.path.dirname(os.path.abspath(__file__)) + '/meshes/')
 
         print(self._parser.get_string(pretty_format=True))
 
@@ -562,14 +564,13 @@ class Mujoco(Simulator):
         """
         # parse sdf file
         sdf_parser = SDFParser(filename=filename)
-        mujoco_generator = MuJoCoParser()
 
-        # generate XML elements
-        elements = [mujoco_generator.generate(tree) for tree in sdf_parser.world.trees]
+        for tree in sdf_parser.world.trees:
+            # # update the position and orientation
+            # tree.position = position
+            # tree.orientation = orientation
 
-        # append each element to worldbody
-        for element in elements:
-            self._worldbody.append(element)
+            self._parser.add_multibody(tree)
 
     def load_mjcf(self, filename, scaling=1., *args, **kwargs):
         """Load a Mujoco file in the simulator.
@@ -589,15 +590,17 @@ class Mujoco(Simulator):
         # self.model = mujoco.load_model_from_path(filename)
         # self.sim = mujoco.MjSim(self.model)
 
-        # parse MJCF file
-        parser = MuJoCoParser(filename=filename)
+        raise NotImplementedError
 
-        # generate XML elements
-        elements = [parser.generate(tree) for tree in parser.world.trees]
-
-        # append each element to worldbody
-        for element in elements:
-            self._worldbody.append(element)
+        # # parse MJCF file
+        # parser = MuJoCoParser(filename=filename)
+        #
+        # # generate XML elements
+        # elements = [parser.generate(tree) for tree in parser.world.trees]
+        #
+        # # append each element to worldbody
+        # for element in elements:
+        #     self._worldbody.append(element)
 
     def load_mesh(self, filename, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), color=None,
                   with_collision=True, flags=None, *args, **kwargs):

pyrobolearn/utils/parsers/robots/data_structures.py

@@ -255,6 +255,21 @@ class Frame(object):
             else:
                 raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray")
 
+    @property
+    def homogeneous(self):
+        """Return the homogeneous matrix based on the position and orientation. Note that if the orientation is
+        None it will be set to the identity matrix, and if the position is None, it will be set to the zero vector."""
+        R = self.rot if self._orientation is not None else np.identity(3)
+        p = self._position if self._position is not None else np.zeros(3)
+        return np.vstack((np.hstack((R, p.reshape(-1, 1))),
+                          np.array([[0, 0, 0, 1]])))
+
+    @homogeneous.setter
+    def homogeneous(self, matrix):
+        """Set the position and orientation of the frame based on the given homogeneous matrix."""
+        self.position = matrix[:3, 3]
+        self.orientation = matrix[:3, :3]
+
 
 class Physics(object):
     r"""Physical properties of the world.
@@ -431,6 +446,17 @@ class World(object):
         """Set the world frame pose."""
         self.frame.pose = pose
 
+    @property
+    def homogeneous(self):
+        """Return the frame homogeneous matrix. Note that if the orientation is None it will be set to the identity
+        matrix, and if the position is None, it will be set to the zero vector."""
+        return self.frame.homogeneous
+
+    @homogeneous.setter
+    def homogeneous(self, matrix):
+        """Set the given homogeneous matrix."""
+        self.frame.homogeneous = matrix
+
 
 class Light(object):
     r"""Light data structure.
@@ -559,6 +585,17 @@ class Light(object):
         """Return the light frame pose."""
         return self.frame.pose
 
+    @property
+    def homogeneous(self):
+        """Return the frame homogeneous matrix. Note that if the orientation is None it will be set to the identity
+        matrix, and if the position is None, it will be set to the zero vector."""
+        return self.frame.homogeneous
+
+    @homogeneous.setter
+    def homogeneous(self, matrix):
+        """Set the given homogeneous matrix."""
+        self.frame.homogeneous = matrix
+
     @pose.setter
     def pose(self, pose):
         """Set the light frame pose."""
@@ -714,6 +751,17 @@ class Floor(object):
         """Set the floor frame pose."""
         self.frame.pose = pose
 
+    @property
+    def homogeneous(self):
+        """Return the frame homogeneous matrix. Note that if the orientation is None it will be set to the identity
+        matrix, and if the position is None, it will be set to the zero vector."""
+        return self.frame.homogeneous
+
+    @homogeneous.setter
+    def homogeneous(self, matrix):
+        """Set the given homogeneous matrix."""
+        self.frame.homogeneous = matrix
+
     @property
     def color(self):
         """Return the ambient color."""
@@ -782,7 +830,10 @@ class MultiBody(object):
     @property
     def root(self):
         """Return the root body element."""
-        return self._root
+        if self._root is not None:
+            return self._root
+        if len(self.bodies):
+            return next(iter(self.bodies))  # get first element
 
     @root.setter
     def root(self, root):
@@ -847,6 +898,17 @@ class MultiBody(object):
         """Set the tree frame pose."""
         self.frame.pose = pose
 
+    @property
+    def homogeneous(self):
+        """Return the homogeneous matrix. Note that if the orientation is None it will be set to the identity
+        matrix, and if the position is None, it will be set to the zero vector."""
+        return self.frame.homogeneous
+
+    @homogeneous.setter
+    def homogeneous(self, matrix):
+        """Set the given homogeneous matrix."""
+        self.frame.homogeneous = matrix
+
 
 # alias
 Tree = MultiBody
@@ -1113,6 +1175,17 @@ class Body(object):
         """Set the body frame pose."""
         self.frame.pose = pose
 
+    @property
+    def homogeneous(self):
+        """Return the homogeneous matrix. Note that if the orientation is None it will be set to the identity
+        matrix, and if the position is None, it will be set to the zero vector."""
+        return self.frame.homogeneous
+
+    @homogeneous.setter
+    def homogeneous(self, matrix):
+        """Set the given homogeneous matrix."""
+        self.frame.homogeneous = matrix
+
     def add_collision(self, collision):
         """
         Add a collision shape to the list of collision shapes.
@@ -1214,6 +1287,12 @@ class Joint(object):
     - SDF: ball, fixed, gearbox, prismatic, revolute, revolute2, screw, universal
     - Dart: ball, free (=floating), euler, prismatic, weld (=fixed), revolute, universal
     - MuJoCo: ball, free (=floating), hinge (=revolute), slide (=prismatic)
+
+    By default, we follow the convention expressed in URDF to describe the frames. That is, the child joint frame is
+    described with respect to the parent joint/link frame.
+
+    References:
+        - [1] http://wiki.ros.org/urdf/XML/joint
     """
 
     def __init__(self, joint_id, name=None, dtype=None, limits=None, parent=None, child=None, axis=None,
@@ -1404,6 +1483,17 @@ class Joint(object):
         """Set the joint frame pose."""
         self.frame.pose = pose
 
+    @property
+    def homogeneous(self):
+        """Return the frame homogeneous matrix. Note that if the orientation is None it will be set to the identity
+        matrix, and if the position is None, it will be set to the zero vector."""
+        return self.frame.homogeneous
+
+    @homogeneous.setter
+    def homogeneous(self, matrix):
+        """Set the given homogeneous matrix."""
+        self.frame.homogeneous = matrix
+
     @property
     def friction(self):
         """Return the joint friction coefficient."""
@@ -1671,7 +1761,9 @@ class Inertia(object):
 class Inertial(object):
     r"""Inertial properties.
 
-    The inertial element groups the mass, inertia, and the body CoM position and orientation.
+    The inertial element groups the mass, inertia, and the body CoM position and orientation. By default, we follow
+    the convention expressed in URDF to describe the frames. That is, the inertial frame is described with respect to
+    the link frame.
 
     Moments of inertia of popular shapes:
 
@@ -1687,6 +1779,9 @@ class Inertial(object):
     - sphere: I = 2./5 * mass * radius**2 * np.ones(3)
     - mesh: use ``trimesh`` library, after loading the mesh, you can access the moments of inertia with
       ``mesh.moment_inertia``.
+
+    References:
+        - [1] http://wiki.ros.org/urdf/XML/link
     """
 
     def __init__(self, mass=None, inertia=None, position=(0., 0., 0.), orientation=None):
@@ -1829,6 +1924,17 @@ class Inertial(object):
         """Set the inertial pose."""
         self.frame.pose = pose
 
+    @property
+    def homogeneous(self):
+        """Return the frame homogeneous matrix. Note that if the orientation is None it will be set to the identity
+        matrix, and if the position is None, it will be set to the zero vector."""
+        return self.frame.homogeneous
+
+    @homogeneous.setter
+    def homogeneous(self, matrix):
+        """Set the given homogeneous matrix."""
+        self.frame.homogeneous = matrix
+
     @staticmethod
     def compute_mass_from_density(shape, dimensions=None, density=1000, volume=None, mesh=None):
         """
@@ -2076,7 +2182,14 @@ Shape = Geometry
 
 
 class Visual(object):
-    r"""visual parameters for body."""
+    r"""visual parameters for body.
+
+    By default, we follow the convention expressed in URDF to describe the frames. That is, the visual frame is
+    described with respect to the link frame.
+
+    References:
+        - [1] http://wiki.ros.org/urdf/XML/link
+    """
 
     def __init__(self, name=None, dtype=None, size=None, color=None, filename=None, position=None, orientation=None,
                  material_name=None, texture=None, diffuse=None, specular=None, emissive=None):
@@ -2230,9 +2343,27 @@ class Visual(object):
         """Set the visual frame pose."""
         self.frame.pose = pose
 
+    @property
+    def homogeneous(self):
+        """Return the frame homogeneous matrix. Note that if the orientation is None it will be set to the identity
+        matrix, and if the position is None, it will be set to the zero vector."""
+        return self.frame.homogeneous
+
+    @homogeneous.setter
+    def homogeneous(self, matrix):
+        """Set the given homogeneous matrix."""
+        self.frame.homogeneous = matrix
+
 
 class Collision(object):
-    r"""Collision parameters for body."""
+    r"""Collision parameters for body.
+
+    By default, we follow the convention expressed in URDF to describe the frames. That is, the collision frame is
+    described with respect to the link frame.
+
+    References:
+        - [1] http://wiki.ros.org/urdf/XML/link
+    """
 
     def __init__(self, name=None, dtype=None, size=None, filename=None, position=None, orientation=None):
         """
@@ -2344,6 +2475,17 @@ class Collision(object):
         """Set the pose of the collision frame."""
         self.frame.pose = pose
 
+    @property
+    def homogeneous(self):
+        """Return the frame homogeneous matrix. Note that if the orientation is None it will be set to the identity
+        matrix, and if the position is None, it will be set to the zero vector."""
+        return self.frame.homogeneous
+
+    @homogeneous.setter
+    def homogeneous(self, matrix):
+        """Set the given homogeneous matrix."""
+        self.frame.homogeneous = matrix
+
 
 class Material(object):
     r"""Material info.

pyrobolearn/utils/parsers/robots/mujoco_parser.py

@@ -11,12 +11,14 @@ References:
 """
 
 import os
+import copy
 import numpy as np
 import xml.etree.ElementTree as ET
 
 # import mesh converter (from .obj to .stl)
 try:
     import trimesh  # https://pypi.org/project/trimesh/
+    from trimesh.exchange.export import export_mesh
 
     # import pymesh    # rapid prototyping platform focused on geometry processing
     # doc: https://pymesh.readthedocs.io/en/latest/user_guide.html
@@ -30,7 +32,8 @@ except ImportError as e:
 from pyrobolearn.utils.parsers.robots.world_parser import WorldParser
 from pyrobolearn.utils.parsers.robots.data_structures import Simulator, World, Tree, Body, Joint, Inertial, \
     Visual, Collision, Light, Floor
-from pyrobolearn.utils.transformation import rotation_matrix_x, rotation_matrix_y, rotation_matrix_z
+from pyrobolearn.utils.transformation import rotation_matrix_x, rotation_matrix_y, rotation_matrix_z, \
+    get_inverse_homogeneous
 
 
 __author__ = "Brian Delhaisse"
@@ -105,6 +108,9 @@ class MuJoCoParser(WorldParser):
         self.create_root("mujoco")
         # add compiler
         self.add_element("compiler", self._root, attributes={'coordinate': 'local', 'angle': 'radian'})
+        # add size
+        self.nconmax = 200  # increase this if necessary (this depends on how many models are loaded)
+        self.add_element("size", self._root, attributes={"nconmax": str(self.nconmax)})
         # add worldbody
         self.worldbody = self.add_element(name="worldbody", parent_element=self.root)
 
@@ -118,6 +124,11 @@ class MuJoCoParser(WorldParser):
         # self._geom_cnt = 0
         # self._site_cnt = 0
 
+        # save homogenous matrices (for later)
+        self._h_bodies, self._h_joints, self._h_visuals, self._h_collisions, self._h_inertials = {}, {}, {}, {}, {}
+        self._assets_tmp = set([])
+        self._mesh_dirname = ''
+
     #################
     # Utils methods #
     #################
@@ -1157,12 +1168,52 @@ class MuJoCoParser(WorldParser):
             raise TypeError("Expecting the given 'tree' to be an instance of `Tree`, but got instead: "
                             "{}".format(type(tree)))
 
+        # update the body and joint positions because in MuJoCo "all elements in defined in the kinematic tree are
+        # expressed in local coordinates, relative to the parent body for bodies, and relative to the body that owns
+        # the element for geoms, joints, sites, cameras and lights", and a joint defined in a body connects that body
+        # with its parent body.
+
+        h_bodies, h_joints, h_visuals, h_collisions, h_inertials = {}, {}, {}, {}, {}
+        for i, body in enumerate(tree.bodies.values()):
+            print(body.name, body.homogeneous)
+            h_inv = get_inverse_homogeneous(body.homogeneous)  # get link/joint frame
+            for visual in body.visuals:  # because geom is described wrt body frame and not link/joint frame
+                h_visuals[visual] = h_inv.dot(visual.homogeneous)
+            for collision in body.collisions:  # because geom is described wrt body frame and not link/joint frame
+                h_collisions[collision] = h_inv.dot(collision.homogeneous)
+            for inertial in body.inertials:  # because inertial is described wrt body frame and not link/joint frame
+                h_inertials[inertial] = h_inv.dot(inertial.homogeneous)
+            for joint in body.joints.values():
+                if joint.child is not None:
+                    h_child = joint.child.homogeneous
+                    h = h_inv.dot(joint.homogeneous).dot(h_child)
+                    h_bodies[joint.child] = h  # because child body is described wrt parent body frame in MJC
+                    h_joints[joint] = get_inverse_homogeneous(h_child)  # because joint are wrt child body frame in MJC
+
+        self._h_bodies, self._h_joints = h_bodies, h_joints
+        self._h_visuals, self._h_collisions, self._h_inertials = h_visuals, h_collisions, h_inertials
+
+        # TODO: WARNING - this modify the given Tree!!!
+        for body, homogeneous in h_bodies.items():
+            body.homogeneous = homogeneous
+        for joint, homogeneous in h_joints.items():
+            joint.homogeneous = homogeneous
+        for visual, homogeneous in h_visuals.items():
+            visual.homogeneous = homogeneous
+        for collision, homogeneous in h_collisions.items():
+            collision.homogeneous = homogeneous
+        for inertial, homogeneous in h_inertials.items():
+            inertial.homogeneous = homogeneous
+
         # generate bodies (inertial, visual, collision) and joints in a recursive manner
         body_tag = self.generate_body(parent_tag, body=tree.root, root=root)
+
+        # empty the temporary assets
+        self._assets_tmp = set([])
+
         return body_tag
 
-    @staticmethod
-    def _convert_mesh(filename):
+    def _convert_mesh(self, filename):
         """
         Convert mesh (from any format) to an STL mesh format. This is because MuJoCo only accepts STL meshes.
 
@@ -1178,14 +1229,22 @@ class MuJoCoParser(WorldParser):
         extension = filename.split('.')[-1]
         if extension.lower() != 'stl':
             # create filename with the correction extension (STL)
-            filename_without_extension = ''.join(filename.split('.')[:-1])
-            new_filename = filename_without_extension + '.stl'
+            dirname = os.path.dirname(filename)
+            basename = os.path.basename(filename)
+            basename_without_extension = ''.join(basename.split('.')[:-1])
+            # filename_without_extension = dirname + basename_without_extension
+            # new_filename = filename_without_extension + '.stl'
+            new_filename = self._mesh_dirname + '/' + basename_without_extension + '.stl'
 
-            # if file do not already exists, convert it
+            # if file does not already exists, convert it
             if not os.path.isfile(new_filename):
-                scene = pyassimp.load(filename)
-                pyassimp.export(scene, new_filename, file_type='stl')
-                pyassimp.release(scene)
+
+                # # Arf, pyassimp export an ASCII STL, but Mujoco requires a binary STL --> use trimesh
+                # scene = pyassimp.load(filename)
+                # pyassimp.export(scene, new_filename, file_type='stl')
+                # pyassimp.release(scene)
+
+                export_mesh(trimesh.load(filename), new_filename)
 
             return new_filename
         return filename
@@ -1300,12 +1359,14 @@ class MuJoCoParser(WorldParser):
                     if asset_tag is None:
                         asset_tag = ET.SubElement(root, "asset")
 
-                    # create mesh tag
+                    # create <mesh> tag in <asset>
                     mesh_path = self._convert_mesh(collision.filename)  # convert to STL if necessary
                     mesh_name = os.path.basename(mesh_path).split('.')[0]
-                    attrib = {'name': mesh_name, 'file': mesh_path}
-                    self._update_attribute_dict(attrib, collision, 'size', key='scale')
-                    ET.SubElement(asset_tag, "mesh", attrib=attrib)
+                    if mesh_name not in self._assets_tmp:  # if the <mesh> doesn't already exists
+                        self._assets_tmp.add(mesh_name)
+                        attrib = {'name': mesh_name, 'file': mesh_path}
+                        self._update_attribute_dict(attrib, collision, 'size', key='scale')
+                        ET.SubElement(asset_tag, "mesh", attrib=attrib)
 
                     # set the mesh asset name
                     geom.attrib["mesh"] = mesh_name
@@ -1325,13 +1386,13 @@ class MuJoCoParser(WorldParser):
 
             # check type and change size
             if 'type' in attrib and 'size' in attrib:
-                if collision.dtype == 'box':  # divide by 2 the dimensions
-                    attrib['size'] = str(np.asarray(collision.size) / 2.)[1:-1]
-                elif collision.dtype == 'cylinder' or collision.dtype == 'capsule':  # divide by 2 the height
-                    radius, length = collision.size
+                if visual.dtype == 'box':  # divide by 2 the dimensions
+                    attrib['size'] = str(np.asarray(visual.size) / 2.)[1:-1]
+                elif visual.dtype == 'cylinder' or visual.dtype == 'capsule':  # divide by 2 the height
+                    radius, length = visual.size
                     attrib['size'] = str(np.asarray([radius, length / 2]))[1:-1]
-                elif collision.dtype == 'mesh':  # set the scale
-                    attrib['fitscale'] = str(collision.size)
+                elif visual.dtype == 'mesh':  # set the scale
+                    attrib['fitscale'] = str(visual.size)
 
             # create <geom> tag
             geom = ET.SubElement(body_tag, "geom", attrib=attrib)
@@ -1346,11 +1407,13 @@ class MuJoCoParser(WorldParser):
                     asset_tag = ET.SubElement(root, "asset")
 
                 # create mesh tag
-                mesh_path = self._convert_mesh(collision.filename)  # convert to STL if necessary
+                mesh_path = self._convert_mesh(visual.filename)  # convert to STL if necessary
                 mesh_name = os.path.basename(mesh_path).split('.')[0]
-                attrib = {'name': mesh_name, 'file': mesh_path}
-                self._update_attribute_dict(attrib, collision, 'size', key='scale')
-                ET.SubElement(asset_tag, "mesh", attrib=attrib)
+                if mesh_name not in self._assets_tmp:  # if the mesh doesn't already exists
+                    self._assets_tmp.add(mesh_name)
+                    attrib = {'name': mesh_name, 'file': mesh_path}
+                    self._update_attribute_dict(attrib, visual, 'size', key='scale')
+                    ET.SubElement(asset_tag, "mesh", attrib=attrib)
 
                 # set the mesh asset name
                 geom.attrib["mesh"] = mesh_name
@@ -1362,7 +1425,6 @@ class MuJoCoParser(WorldParser):
 
         # create <joint>
         for joint in body.parent_joints.values():  # parent_joints
-            print("Joint...")
             self.generate_joint(body_tag, joint)
 
         # create inner <body>
@@ -1421,13 +1483,14 @@ class MuJoCoParser(WorldParser):
         if 'type' in attrib:
             return ET.SubElement(parent_tag, "joint", attrib=attrib)
 
-    def add_multibody(self, tree):
+    def add_multibody(self, tree, mesh_directory_path=''):
         r"""
         Add the given tree / multi-body data structure.
 
         Args:
             tree (Tree, Body): multi-body data structure. If it is a body instance, it will automatically be
               wrapped in a Tree instance.
+            mesh_directory_path (str): path to the mesh directory to add the converted meshes.
 
         Returns:
             ET.Element: body XML element
@@ -1439,6 +1502,8 @@ class MuJoCoParser(WorldParser):
                 raise TypeError("Expecting the given 'tree' to be an instance of `Tree` or `Body` but got "
                                 "instead: {}".format(type(tree)))
 
+        self._mesh_dirname = mesh_directory_path if isinstance(mesh_directory_path, str) else ''
+
         # generate tree
         return self.generate_tree(parent_tag=self.worldbody, tree=tree, root=self.root)
 

pyrobolearn/utils/parsers/robots/robot_parser.py

@@ -2,11 +2,13 @@
 """Define the abstract Robot parser.
 """
 
+import os
+
 # import XML parser
 import xml.etree.ElementTree as ET
 from xml.dom import minidom  # to print in a pretty way the XML file
 
-from pyrobolearn.utils.parsers.robots.data_structures import Tree
+from pyrobolearn.utils.parsers.robots.data_structures import MultiBody
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2019, PyRoboLearn"
@@ -31,7 +33,9 @@ class RobotParser(object):
         self.root = None
         self.tree = None
         self.filename = filename
+        self.dirname = ''
         if filename is not None:
+            self.dirname = os.path.dirname(filename) + '/'
             self.parse(filename)
 
     @property
@@ -51,7 +55,7 @@ class RobotParser(object):
 
     @tree.setter
     def tree(self, tree):
-        if tree is not None and not isinstance(tree, Tree):
+        if tree is not None and not isinstance(tree, MultiBody):
             raise TypeError("Expecting the given tree to be an instance of `Tree`, but got instead: "
                             "{}".format(type(tree)))
         self._tree = tree
@@ -70,7 +74,7 @@ class RobotParser(object):
         Return the Tree containing all the elements.
 
         Returns:
-            Tree: tree data structure.
+            MultiBody: tree data structure.
         """
         return self.tree
 
@@ -79,7 +83,7 @@ class RobotParser(object):
         Generate the XML tree from the `Tree` data structure.
 
         Args:
-            tree (Tree): Tree data structure.
+            tree (MultiBody): Tree data structure.
 
         Returns:
             ET.Element: root element in the XML file.

pyrobolearn/utils/parsers/robots/urdf_parser.py

@@ -66,7 +66,7 @@ class URDFParser(RobotParser):
                 material.color = color_tag.attrib.get('rgba')
             texture_tag = material_tag.find('texture')
             if texture_tag is not None:
-                material.texture = texture_tag.attrib.get('filename')
+                material.texture = self.dirname + texture_tag.attrib.get('filename')
             tree.materials[material.name] = material
 
         # check bodies / links
@@ -104,8 +104,7 @@ class URDFParser(RobotParser):
 
         return tree
 
-    @staticmethod
-    def _check_body(tree, body_tag, idx):
+    def _check_body(self, tree, body_tag, idx):
         """
         Return Body instance from a <link> tag.
 
@@ -174,7 +173,7 @@ class URDFParser(RobotParser):
                         elif dtype == 'cylinder':
                             visual.size = (geometry_type_tag.attrib['radius'], geometry_type_tag.attrib['length'])
                         elif dtype == 'mesh':
-                            visual.filename = geometry_type_tag.attrib['filename']
+                            visual.filename = self.dirname + geometry_type_tag.attrib['filename']
                             visual.size = geometry_type_tag.attrib.get('scale')
 
             # material
@@ -189,7 +188,7 @@ class URDFParser(RobotParser):
                     if color is not None:
                         material.color = color.attrib['rgba']
                     elif texture is not None:
-                        material.texture = texture.attrib['filename']
+                        material.texture = self.dirname + texture.attrib['filename']
                 else:
                     material = tree.materials.get(name)
                 visual.material = material
@@ -226,7 +225,7 @@ class URDFParser(RobotParser):
                         elif dtype == 'cylinder':
                             collision.size = (geometry_type_tag.attrib['radius'], geometry_type_tag.attrib['length'])
                         elif dtype == 'mesh':
-                            collision.filename = geometry_type_tag.attrib['filename']
+                            collision.filename = self.dirname + geometry_type_tag.attrib['filename']
                             collision.size = geometry_type_tag.attrib.get('scale')
 
             # set collision to body

pyrobolearn/utils/transformation.py

@@ -95,6 +95,37 @@ def get_homogeneous_transform(position, orientation):
     return H
 
 
+def get_inverse_homogeneous(matrix):
+    r"""
+    Return the inverse of the homogeneous matrix.
+
+    If the homogeneous matrix is expressed as:
+
+    .. math::
+
+        H = [[R, p],
+             [zeros(3), 1]],
+
+    where :math:`R` is the 3x3 rotation matrix, :math:`p` is the 3x1 position vector. Then, the inverse homogeneous
+    matrix is given by:
+
+    .. math::
+
+        H^{-1} = [[R^\top, -R^\top p],
+                  [zeros(3), 1]].
+
+    Args:
+        matrix (np.array[float[4,4]]): homogeneous matrix to inverse.
+
+    Returns:
+        np.array[float[4,4]]: inverse homogeneous matrix.
+    """
+    R = matrix[:3, :3].T
+    p = -R.dot(matrix[:3, 3].reshape(-1, 1))
+    return np.vstack((np.hstack((R, p)),
+                      np.array([[0, 0, 0, 1]])))
+
+
 def homogeneous_to_pose(matrix):
     r"""
     Return a pose (7D vector: position + quaternion) from a homogeneous matrix.