Prototype

.github/workflows/ci.yml

@@ -28,11 +28,23 @@ jobs:
         sudo apt-get install -y libsdformat13-dev python3-ignition-math7 python3-sdformat13
     - name: Install test dependencies
       run: python3 -m pip install -U tox
-    - name: Test
+    - name: Test sdformat_mjcf_utils
       env:
         PYTHONPATH: ${{ env.pythonLocation }}/lib/python${{ matrix.python }}/site-packages:/usr/lib/python3/dist-packages
       run: |
-        cd sdformat_mjcf
+        cd sdformat_mjcf_utils
+        python3 -m tox -e py
+    - name: Test sdformat_to_mjcf
+      env:
+        PYTHONPATH: ${{ env.pythonLocation }}/lib/python${{ matrix.python }}/site-packages:/usr/lib/python3/dist-packages
+      run: |
+        cd sdformat_to_mjcf
+        python3 -m tox -e py
+    - name: Test mjcf_to_sdformat
+      env:
+        PYTHONPATH: ${{ env.pythonLocation }}/lib/python${{ matrix.python }}/site-packages:/usr/lib/python3/dist-packages
+      run: |
+        cd mjcf_to_sdformat
         python3 -m tox -e py
     - name: Upload Coverage to Codecov
       uses: codecov/codecov-action@v2

mjcf_to_sdformat/LICENSE

@@ -0,0 +1,16 @@
+Software License Agreement (Apache License)
+
+Copyright 2022 Open Source Robotics Foundation
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+

mjcf_to_sdformat/README.md

@@ -0,0 +1,54 @@
+# Tools for converting between MJCF and SDFormat
+
+To start development, create a python3 virtual environment, upgrade pip and
+install dm-control
+
+```
+python3 -m venv path/to/venv --system-site-packages
+. path/to/venv/bin/activate
+
+pip install -U pip
+pip install dm-control
+```
+
+Install `python3-ignition-math7` from the
+[nightly](https://gazebosim.org/docs/all/release#type-of-releases) repo.
+
+Build `libsdformat` from source from the `ahcorde/python/all` branch and add
+the installation path to your `LD_LIBRARY_PATH` and `PYTHONPATH`.
+
+```
+export LD_LIBRARY_PATH="$LD_LIBRARY_PATH:$INSTALL_DIR/lib"
+export PYTHONPATH="$PYTHONPATH:$INSTALL_DIR/lib/python"
+```
+
+where `$INSTALL_DIR` is the installation directory you used when building
+libsdformat.
+
+Install the `mjcf_to_sdformat` packages in "editable" mode
+
+```
+pip install -e path/to/mjcf_to_sdformat
+```
+
+To convert an SDFormat file to mjcf:
+
+```
+python -m mjcf_to_sdformat.mjcf2sdformat path/to/file.xml | tee new_file.sdf
+```
+
+To run the sdf file in GazeboSim, follow [this instructions to install Gazebo Sim](https://gazebosim.org/docs/latest/install)
+
+```
+ign gazebo new_file.xml
+```
+
+### Running tests
+
+To run tests, either run the test files individually, eg:
+
+```
+python tests/test_add_geometry.py
+```
+
+or run `pytest` from the root directory.

mjcf_to_sdformat/mjcf_to_sdformat/converters/geometry.py

@@ -0,0 +1,139 @@
+# Copyright (C) 2022 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, version 2.0 (the "License")
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#       http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Module to convert MJCF geoms to SDFormat Collision/Visual geometries"""
+
+from ignition.math import Pose3d, Quaterniond, Vector2d, Vector3d
+
+import sdformat as sdf
+import sdformat_mjcf_utils.sdf_utils as su
+
+VISUAL_NUMBER = 0
+COLLISION_NUMBER = 0
+
+
+def add_mjcf_geometry_to_sdf(geom):
+    """
+    Converts an MJCF geom to an SDFormat geometry.
+
+    :param mjcf.Element geom: The MJCF geom
+    :return: The newly created SDFormat geometry.
+    :rtype: sdf.Geometry
+    """
+    if geom.root.default.geom is not None:
+        geom_attributes = geom.get_attributes().items()
+        for k, v in geom.root.default.geom.get_attributes().items():
+            try:
+                geom.get_attributes()[k]
+            except:
+                geom.set_attributes(**{k:v})
+    geom_attributes = geom.get_attributes().items()
+
+    sdf_geometry = sdf.Geometry()
+    if geom.type == "box":
+        box = sdf.Box()
+        box.set_size(su.list_to_vec3d(geom.size))
+        sdf_geometry.set_box_shape(box)
+        sdf_geometry.set_type(sdf.Geometry.GeometryType.BOX)
+    elif geom.type == "capsule":
+        capsule = sdf.Capsule()
+        capsule.set_radius(geom.size[0])
+        if geom.fromto is None:
+            capsule.set_length(geom.size[1])
+        else:
+            v1 = Vector3d(geom.fromto[0], geom.fromto[1], geom.fromto[2])
+            v2 = Vector3d(geom.fromto[3], geom.fromto[4], geom.fromto[5])
+            length = v1.distance(v2)
+            capsule.set_length(length)
+        sdf_geometry.set_capsule_shape(capsule)
+        sdf_geometry.set_type(sdf.Geometry.GeometryType.CAPSULE)
+    elif geom.type == "cylinder":
+        cylinder = sdf.Cylinder()
+        cylinder.set_radius(geom.size[0])
+        if geom.fromto is None:
+            cylinder.set_length(geom.size[1] * 2)
+        else:
+            v1 = Vector3d(geom.fromto[0], geom.fromto[1], geom.fromto[2])
+            v2 = Vector3d(geom.fromto[3], geom.fromto[4], geom.fromto[5])
+            length = v1.distance(v2)
+            cylinder.set_length(length)
+        sdf_geometry.set_cylinder_shape(cylinder)
+        sdf_geometry.set_type(sdf.Geometry.GeometryType.CYLINDER)
+    elif geom.type == "ellipsoid":
+        ellipsoid = sdf.Ellipsoid()
+        ellipsoid.set_radii(su.list_to_vec3d(geom.size))
+        sdf_geometry.set_ellipsoid_shape(ellipsoid)
+        sdf_geometry.set_type(sdf.Geometry.GeometryType.ELLIPSOID)
+    elif geom.type == "sphere":
+        sphere = sdf.Sphere()
+        sphere.set_radius(geom.size[0])
+        sdf_geometry.set_sphere_shape(sphere)
+        sdf_geometry.set_type(sdf.Geometry.GeometryType.SPHERE)
+    elif geom.type == "plane":
+        plane = sdf.Plane()
+        plane.set_size(Vector2d(geom.size[0] * 2, geom.size[1] * 2))
+        sdf_geometry.set_plane_shape(plane)
+        sdf_geometry.set_type(sdf.Geometry.GeometryType.PLANE)
+    else:
+        raise RuntimeError(
+            f"Encountered unsupported shape type {geom.type}")
+    return sdf_geometry
+
+
+def add_mjcf_visual_to_sdf(geom):
+    """
+    Converts MJCF geom to a SDFormat visual
+    MJCF geom should be part of group `VISUAL_GEOM_GROUP`.
+
+    :param mjcf.Element geom: The MJCF geom.
+    :return: The newly created SDFormat visual.
+    :rtype: sdformat.Visual
+    """
+    global VISUAL_NUMBER
+    visual = sdf.Visual()
+    if geom.name is not None:
+        visual.set_name("visual_" + geom.name)
+    else:
+        visual.set_name("visual_" + str(VISUAL_NUMBER))
+        VISUAL_NUMBER = VISUAL_NUMBER + 1
+    sdf_geometry = add_mjcf_geometry_to_sdf(geom)
+    if sdf_geometry is not None:
+        visual.set_geometry(sdf_geometry)
+    else:
+        return None
+    return visual
+
+
+def add_mjcf_collision_to_sdf(geom):
+    """
+    Converts MJCF geom to a SDFormat collision
+    MJCF geom should be part of group `COLLISION_GEOM_GROUP`.
+
+    :param mjcf.Element geom: The MJCF geom.
+    :return: The newly created SDFormat collision.
+    :rtype: sdformat.Collision
+    """
+    global COLLISION_NUMBER
+    col = sdf.Collision()
+    if geom.name is not None:
+        col.set_name("collision_" + geom.name)
+    else:
+        col.set_name("collision_" + str(COLLISION_NUMBER))
+        COLLISION_NUMBER = COLLISION_NUMBER + 1
+    sdf_geometry = add_mjcf_geometry_to_sdf(geom)
+    if sdf_geometry is not None:
+        col.set_geometry(sdf_geometry)
+    else:
+        return None
+    return col

mjcf_to_sdformat/mjcf_to_sdformat/converters/joint.py

@@ -0,0 +1,55 @@
+# Copyright (C) 2022 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, version 2.0 (the "License")
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#       http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ignition.math import Pose3d, Vector3d, Quaterniond
+
+import sdformat as sdf
+
+
+def add_mjcf_joint_to_sdf(joint, parent_name, child_name):
+    if joint.root.default.joint is not None:
+        joint_attributes = joint.get_attributes().items()
+        for k, v in joint.root.default.joint.get_attributes().items():
+            try:
+                joint.get_attributes()[k]
+            except:
+                joint.set_attributes(**{k:v})
+        # joint.set_attributes(**joint.root.default.joint.get_attributes())
+    joint_sdf = sdf.Joint()
+    joint_axis_sdf = sdf.JointAxis()
+    if joint.type == "hinge":
+        joint_sdf.set_type(sdf.Joint.JointType.REVOLUTE)
+        # joint_sdf.set_raw_pose(Posed3d(Vector3d))
+        errors = joint_axis_sdf.set_xyz(Vector3d(joint.axis[0],
+                                                 joint.axis[1],
+                                                 joint.axis[2]))
+        joint_sdf.set_name(joint.name)
+        if parent_name is None:
+            joint_sdf.set_parent_link_name("world")
+        else:
+            joint_sdf.set_parent_link_name(parent_name)
+
+        joint_sdf.set_child_link_name(child_name)
+        geom = joint.root.find("geom", child_name)
+        if geom is not None and geom.fromto is not None:
+            v1 = Vector3d(geom.fromto[0], geom.fromto[1], geom.fromto[2])
+            v2 = Vector3d(geom.fromto[3], geom.fromto[4], geom.fromto[5])
+            joint_sdf.set_raw_pose(Pose3d(Vector3d(-(v2 - v1) / 2),
+                                          Quaterniond()))
+        if joint.damping is not None:
+            joint_axis_sdf.set_damping(joint.damping)
+        joint_sdf.set_axis(0, joint_axis_sdf)
+        return joint_sdf
+    else:
+        return None

mjcf_to_sdformat/mjcf_to_sdformat/converters/light.py

@@ -0,0 +1,60 @@
+# Copyright (C) 2022 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, version 2.0 (the "License")
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#       http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ignition.math import Color, Pose3d, Vector3d, Quaterniond
+
+import sdformat_mjcf_utils.sdf_utils as su
+
+import sdformat as sdf
+
+LIGHT_NUMBER = 0
+
+def add_mjcf_light_to_sdf(light):
+    light_sdf = sdf.Light()
+    if light.name is not None:
+        light_sdf.set_name(light.name)
+    else:
+        global LIGHT_NUMBER
+        light_sdf.set_name("light_" + str(LIGHT_NUMBER))
+        LIGHT_NUMBER = LIGHT_NUMBER + 1
+
+    if light.pos is not None:
+        light_sdf.set_raw_pose(Pose3d(su.list_to_vec3d(light.pos),
+                                      Quaterniond()))
+    if light.castshadow is not None:
+        light_sdf.set_cast_shadow(bool(light.castshadow))
+
+    if light.attenuation is not None:
+        light_sdf.set_constant_attenuation_factor(light.attenuation[0])
+        light_sdf.set_linear_attenuation_factor(light.attenuation[1])
+        light_sdf.set_quadratic_attenuation_factor(light.attenuation[2])
+
+    if light.diffuse is not None:
+        light_sdf.set_diffuse(Color(light.diffuse[0],
+                                    light.diffuse[1],
+                                    light.diffuse[2]))
+    if light.specular is not None:
+        light_sdf.set_diffuse(Color(light.specular[0],
+                                    light.specular[1],
+                                    light.specular[2]))
+    if light.directional is not None:
+        if light.directional:
+            light_sdf.set_type(sdf.Light.LightType.DIRECTIONAL)
+        else:
+            light_sdf.set_type(sdf.Light.LightType.SPOT)
+
+    if light.dir is not None:
+        light_sdf.set_direction(su.list_to_vec3d(light.dir))
+
+    return light_sdf