Feature Request

As a creator of custom RobotModel for my tools, I want to export them to URDF so that I can import them to PyBullet for planning. At the moment, the RobotModel.to_urdf_file() from #681 works when Links have visual geometry that are primitives, but not meshes.

I have a simple test here. The output has a line <mesh filename="" />, I suspect there there should be some sort of automatic obj export function that should do the exporting in some sort of file structure and link the file names.

Test

from compas.geometry import Box, Circle, Cylinder, Frame, Plane, Vector
from compas.datastructures import Mesh
from compas.robots import Joint, RobotModel

model = RobotModel(name='MinimalRobotModel')

# Link 1 has Box
box_1 = Box(Frame([2, .5, .25], [1, 0, 0], [0, 1, 0]), 1, 2, .5)
box_2 = Box(Frame([2, 0, 4], [1, 0, 0], [0, 1, 0]), .5, 1, 7)
link1 = model.add_link(name='link1', visual_meshes=[box_1, box_2])
# Link 2 has Cylinder
cylinder = Cylinder(Circle(Plane([0, 0, 0], [0, 0, 1]), .5), 8)
link2 = model.add_link(name='link2', visual_meshes=[cylinder])
# Link 3 has Mesh
v, f = Box(Frame([0, .5, .25], [1, 0, 0], [0, 1, 0]), 1, 2, .5).to_vertices_and_faces()
mesh = Mesh.from_vertices_and_faces(v,f)
link3 = model.add_link(name='link3', visual_meshes=[mesh])

origin = Frame([0, 0, 7], [1, 0, 0], [0, 1, 0])
axis = Vector(1, 0, 0)
model.add_joint(
     name='joint1',
     type=Joint.CONTINUOUS,
     parent_link=link1,
     child_link=link2,
     origin=origin,
     axis=axis,
)
model.add_joint(
     name='joint2',
     type=Joint.CONTINUOUS,
     parent_link=link2,
     child_link=link3,
     origin=origin,
     axis=axis,
)

file = 'robot_model.xml'
model.to_urdf_file(file, prettify=True)

The result robot_model.xml

<?xml version="1.0" encoding="utf-8"?>
<robot name="MinimalRobotModel">
  <link name="link1">
    <visual>
      <origin rpy="0.0 -0.0 0.0" xyz="2.0 0.5 0.25" />
      <geometry>
        <box size="1.0 2.0 0.5" />
      </geometry>
      <material name="Material">
        <color rgba="0.8 0.8 0.8 1.0" />
      </material>
    </visual>
    <visual>
      <origin rpy="0.0 -0.0 0.0" xyz="2.0 0.0 4.0" />
      <geometry>
        <box size="0.5 1.0 7.0" />
      </geometry>
      <material name="Material">
        <color rgba="0.8 0.8 0.8 1.0" />
      </material>
    </visual>
  </link>
  <link name="link2">
    <visual>
      <origin rpy="-0.0 -0.0 -1.57079632679" xyz="0.0 0.0 0.0" />
      <geometry>
        <cylinder length="8.0" radius="0.5" />
      </geometry>
      <material name="Material">
        <color rgba="0.8 0.8 0.8 1.0" />
      </material>
    </visual>
  </link>
  <link name="link3">
    <visual>
      <geometry>
        <mesh filename="" />
      </geometry>
      <material name="Material">
        <color rgba="0.8 0.8 0.8 1.0" />
      </material>
    </visual>
  </link>
  <joint name="joint1" type="continuous">
    <parent link="link1" />
    <child link="link2" />
    <axis xyz="1.0 0.0 0.0" />
    <origin rpy="0.0 -0.0 0.0" xyz="0.0 0.0 7.0" />
  </joint>
  <joint name="joint2" type="continuous">
    <parent link="link2" />
    <child link="link3" />
    <axis xyz="1.0 0.0 0.0" />
    <origin rpy="0.0 -0.0 0.0" xyz="0.0 0.0 7.0" />
  </joint>
</robot>

Describe the solution you'd like I want the RobotModel.to_urdf_file() function to be able to export a URDF file together with the link meshes in a folder next to the URDF or in a "package" folder of some sorts.

I've been working on something that's in the direction of what you want and is currently under re-review. https://github.com/compas-dev/compas_fab/pull/259. The idea is that for the life of the PyBulletClient there exists a temporary directory which contains a shadow of the robot loaded to the pybullet server. This shadow is stored as a URDF along with copies of all the associated geometry. The URDF gets updated each time an attached collision mesh is added or removed, and then this new URDF is loaded into pybullet. I don't know if this meets with all of your requirements having a RobotModel as a tool, but it would be nice to get feedback on this feature. The bulk of the code for writing the stls and the urdf is in PyBulletClient.cache_robot, so feel free to steal what you need for your uses. The issue with your suggestion for modifying RobotModel.to_urdf_file is that pybullet does a lot better with the mesh filenames being given as relative paths, but a ROS user would want those file names given as package://... and those packages might already exist and don't need to be exported again, or not, on a mesh by mesh basis. I suppose it's doable, but it seems tricky to make that work well.

pybullet does a lot better with the mesh filenames being given as relative paths, but a ROS user would want those file names given as package://... and those packages might already exist and don't need to be exported again

@beverlylytle Hmm, I think pybullet supports directly parse URDF with package:// path too, and the package can cross-reference each other as long as the packages are saved at the same parent folder.

For example, in the RFL unit test that I wrote for compas_fab_pychoreo, we parse the RFL URDF directly (without modifying the mesh paths therein). The RFL robot needs two ROS packages to work: abb_irb4600_40_255 and rfl_description, both of which I submoduled them here.

Notice that inside the RFL's URDF, it's cross-referencing the meshes in the abb_irb4600_40_255 package.

  <link name="robot11_link_3">
    <inertial>
      <origin xyz="0.07000 -0.26600 0.08800"/>
      <mass value="120.00000"/>
      <inertia ixx="1.34500" ixy="0.55860" ixz="0.18480" iyy="4.65068" iyz="-0.70224" izz="2.76032"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://abb_irb4600_40_255/meshes/visual/link_3.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.7725490 0.7803922 0.7686275 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://abb_irb4600_40_255/meshes/collision/link_3.stl"/>
      </geometry>
    </collision>
  </link>

And pybullet (and our PybulletClient) has no problem parsing them:

Thus, I second @yck011522 's request on the URDF writer export and link meshes. Specifically, we can do the following:

Have the user specify the package name (e.g. xxx_description) and a path to save the package
Create standard ROS robot description package folder structure in the target path
Export meshes (as stl or obj) in xxx_description/meshes, with some auto-generated names, e.g. link_name_obj_[i]_[collision]

Link these mesh file name in the generated URDF, like

  <geometry>
    <mesh filename="package://xxx_description/meshes/collision/link_name_obj_i_collision.stl"/>
  </geometry>

I think this shouldn't be too hard to implement with current compas functionalities? And I don't see the main usage of this feature be exporting a robot URDF from a robot that's parsed from a pre-existing URDF, but rather export a robot package from a custom, user-defined robot (tool), like in Victor's use cases.

as long as the packages are saved at the same parent folder.

Precisely. PyBullet does not treat packages as ROS does. It discards the "package" prefix and treats the rest of the string as a local path as opposed to searching within the ROS_PACKAGE_PATH environment variable. This version of to_urdf_file does not support ROS users. There is a lot of structure within a ROS package. If a ROS user were to load urdf to a robot model that made use of an already existing and complex package (or multiple packages) and then exported this robot model again with this one newly created mock package with only some geometry in it, this could break the user's ROS setup. Even if it's true that this isn't the main use case as you suggest (@yijiangh ), it's still a use case if RobotModel.to_urdf_file is modified in the way suggested above, and needs to be fully supported.

Since this feature is PyBullet specific (and very tricky to generalize to other backends), it should not initially be a part of RobotModel.to_urdf_file until there are the mechanisms handle the use cases for all supported backends. However, it might make sense for there to be a new static method within the PyBulletClient. I am still curious to know if what exists in the above linked PR is sufficient for @yck011522 's purposes or not.

hum.. I have to say I'm a little lost from the discussion that went deeper than what I can handle. But I tend to agree with a static method within PyBulletClient, before similar implementations can be made for other backends.

I didn't know it was not possible with other backends because the way the compas.Robot tutorial was written presented all the methods in creating the RobotModel (and it look great) only after that it didn't mention how this model can be loaded into the backends. Turns out this is actually not possible? Sorry that I cannot make meaningful contribution to this problem but maybe I can comment on how the docs/tutorials can be made more user friendly for new users:

I think it is nice to map out different possible workflow for defining robots and path planning. And I think it is particularly important to mention the dependency needed, as not new users (like myself) may not be aware of the implications to running docker / VM or IPy vs cPy etc. For example I tried the following workflow which worked for me:

Path planning with RFL Robot and non-kinematic tools

RFL and ABB Robot URDF installed as packages in ROS (with a Linux VM hosting the ROS)
compas_fab loading the model from ROS and visualization in Rhino / GH
Attached Collision Mesh to Robot Links passed from compas_fab to MoveIt via ROS.
Tools and Attached Workpiece also passed from compas_fab to MoveIt via ROS.
path planning using MoveIt Backend

Dependency:

Python packages: compas, compas_fab
VMWare Workstation Pro running: Ubuntu 16 (I don't remember the setup instructions)
RFL and ABB packages installed in ROS
Windows on the Rhino 6 side for defining ACM / CM / Tools and for visualization.

What I'm trying to do at the moment with @yijiangh 's help is to achieve another workflow:

Path planning with RFL Robot and Kinematic ToolModels

RFL and ABB Robot / ACM / Tools and Attached Workpiece all passed from compas_fab to PyBullet.
Visualization of IK and planned path using compas.RobotModel functionalities.
path planning using PyBullet Backend

Dependency:

Python packages: compas, compas_fab, compas_fab_pychoreo (experimental from @yijiangh)
Others I'm not sure yet, but I know I do not need VM because PyBullet can run on Windows in cPython.

At the moment, the RobotModel.to_urdf_file() from https://github.com/compas-dev/compas/pull/681 works when Links have visual geometry that are primitives, but not meshes

@yck011522 being able to write URDF's with proper meshes seems super relevant. Just a friendly bump, while clawing my way through the issue tracker

Actually this is solved. We can write URDF packages with meshes exported. Thanks all @beverlylytle @yijiangh @gonzalocasas for all the fishes.

compas-dev / compas

Add URDF Writer support for Links with Meshes #773

Feature Request