Save workcell as URDF #155

audrow · 2023-07-27T18:54:03Z

The current workcell is saved as a URDF when you click "Export URDF" in the "File" menu.

Note this doesn't include mass/inertia or joint data. I'll make separate PRs for those.

Output URDF

<robot name="test_workcell">
  <link name="world">
    <inertial>
      <origin xyz="0 0 0" rpy="0 -0 0" />
      <mass value="0" />
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
    </inertial>
    <visual name="">
      <origin xyz="1.2212629318237305 -2.776434898376465 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://nexus_poc/meshes/SCARA_Table_only.stl" scale="0.009999999776482582 0.009999999776482582 0.009999999776482582" />
      </geometry>
    </visual>
  </link>
  <link name="top_anchor">
    <inertial>
      <origin xyz="-1.0387370586395264 -2.961110830307007 1.2591593265533447" rpy="0 0 1.5707963705062866" />
      <mass value="0" />
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
    </inertial>
  </link>
  <link name="goal_1">
    <inertial>
      <origin xyz="0 0.5 0" rpy="0 0 -1.5707963705062866" />
      <mass value="0" />
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
    </inertial>
  </link>
  <link name="goal_2">
    <inertial>
      <origin xyz="-0.5 0 0" rpy="0 0 -1.5707963705062866" />
      <mass value="0" />
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
    </inertial>
  </link>
</robot>

Workcell

{
  "name": "test_workcell",
  "id": 0,
  "frames": {
    "2": {
      "parent": 0,
      "Pose3D": {
        "trans": [
          -1.038737,
          -2.9611108,
          1.2591593
        ],
        "rot": {
          "yaw": {
            "deg": 90.0
          }
        }
      },
      "name": "top_anchor",
      "mesh_constraint": {
        "entity": 1,
        "element": {
          "Vertex": 0
        },
        "relative_pose": {
          "trans": [
            -2.26,
            -0.18467593,
            1.2591593
          ],
          "rot": {
            "yaw": {
              "deg": 90.0
            }
          }
        }
      }
    },
    "3": {
      "parent": 2,
      "Pose3D": {
        "trans": [
          0.0,
          0.5,
          0.0
        ],
        "rot": {
          "yaw": {
            "deg": -90.0
          }
        }
      },
      "name": "goal_1"
    },
    "4": {
      "parent": 2,
      "Pose3D": {
        "trans": [
          -0.5,
          0.0,
          0.0
        ],
        "rot": {
          "yaw": {
            "deg": -90.0
          }
        }
      },
      "name": "goal_2"
    }
  },
  "visuals": {
    "1": {
      "parent": 0,
      "name": "",
      "geometry": {
        "Mesh": {
          "filename": "package://my_pkg/meshes/robot.stl",
          "scale": [
            0.01,
            0.01,
            0.01
          ]
        }
      },
      "pose": {
        "trans": [
          1.2212629,
          -2.776435,
          0.0
        ],
        "rot": {
          "yaw": {
            "deg": 0.0
          }
        }
      }
    }
  },
  "collisions": {}
}

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

luca-della-vedova · 2023-07-28T03:26:01Z

Very nice! The general approach looks good I see some of the parts are still left to do where:

Visuals / Collisions should be pretty straightforward, since the implementation is pretty much there. They are just children of frames so by doing iteration over children of frames when creating links you should be able to populate them.
Mass and inertial data should be still quite simple, the components are there and they should be deserialized
Joints data is more tricky, I think we would require need to adjust the Joint component to make sure it can hold references to parent and child frames, then we can just export them as they are, at least until we have a more functional implementation.

The first point should be pretty much doable in the current implementation.
The second point will need a bit of thinking as to what is a Frame and what is a Link. In order of increasing complexity we could:

Add Link components to a Frame when loading a Urdf (i.e. here), then when saving add a Option<&Link> to the frame and, if the component is present, serialize its data.
The above plus have a system that adds Link components to all frames, then the query above would just be for &Link and data would always be serialized.
Any of the above but with Link being a new entity that is a child of Frame instead of a component of Frame, to have a bit more encapsulation.
We will also need to add the missing data to the inertial structs.

The third point will be tricky, first of all the Joint struct will have to be updated to have the missing joint data (like type, limits etc) and hold references to child and parent frames. References to entities can be implemented through a generic with types that implement RefTrait as is done in MeshConstraint. The idea is that both u32 and Entity implement RefTrait and we serialize / deserialize u32 while using Entity internally.

Now the tricky part is that some of these points will be hard to do without a functional Load implementation for workcells, so I'm happy to split up the work into separate PRs as you see fit.

The only suggestion I have on the current implementation is on the From function. While From makes conversion quite simple, is has the issue that there is no significant way to encode failure since it is forced to return a urdf_rs::Robot. This means that, if anything went wrong during conversion, you are only left with the choice of panicking, logging an error and continuing, or return an empty structure of sorts to encode failure and leave the function caller to figure out that it went wrong.

By contrast, if you had your own conversion function you could return either an Option<urdf_rs::Robot> or a Result<urdf_rs::Robot, UrdfConversionError>, an example of this being done is when we convert legacy building projects to sites here

audrow · 2023-07-31T13:34:08Z

The only suggestion I have on the current implementation is on the From function. While From makes conversion quite simple, is has the issue that there is no significant way to encode failure since it is forced to return a urdf_rs::Robot

In trying to implement this, it seems that there isn't a reason that the code would fail here. As long as the workcell is correctly defined, it should just be a data mapping, and I think that Rust will stop us if there is a mismatch in the data structures. Otherwise, am I missing something?

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

audrow · 2023-07-31T15:34:13Z

Visuals / Collisions should be pretty straightforward, since the implementation is pretty much there. They are just children of frames so by doing iteration over children of frames when creating links you should be able to populate them.

I added visuals in bb09860 and collisions in ccb7915. The test.workcell.json example doesn't have any collisions, so I haven't been able to test that, but I follow a similar process to what I do for visuals. I updated the example above with the visuals included.

audrow · 2023-07-31T15:36:40Z

Now the tricky part is that some of these points will be hard to do without a functional Load implementation for workcells, so I'm happy to split up the work into separate PRs as you see fit.

Let's add inertias and joint data in a separate PR.

mxgrey

Thanks for this meaty PR, Audrow! It's great to think that we'll be able to export URDFs soon.

I'm leaving a few comments about best practices within Rust that I think will help tighten up the implementation. I'll do a more careful review of the conversion logic after I've gotten through reviewing down some of the earlier site editor PRs.

mxgrey · 2023-07-31T16:34:21Z

rmf_site_format/src/workcell.rs

+                    Angle::Rad(v) => urdf_rs::Vec3([0.0, 0.0, v as f64]),
+                    Angle::Deg(v) => urdf_rs::Vec3([0.0, 0.0, v.to_radians() as f64]),
+                },
+                _ => todo!("Unsupported rotation type for conversion to urdf pose"),


This will cause the code to panic if pose.rot is a Rotation::Quat. That's a big problem since it would be perfectly reasonable for someone to use quaternions to describe a pose.

Another issue with this pattern is if we ever add another variant to Rotation then we'll never notice that we need to update this pattern matching block. We should prefer for this block to produce a compilation error if we ever add another Rotation variant.

So let's get rid of this line and add a pattern matcher for Rotation::Quat(_). We can draw inspiration from the implementation here, except I believe that was implemented before we accepted glam as a dependency. glam is the crate that bevy uses for math, and it's now a dependency for rmf_site_format so we can feel free to use glam for things like this.

Another tip is that we don't need to do any pattern matching for Angle because Angle has a radians() method. E.g. consider this alternative for the first matching arm:

Rotation::EulerExtrinsicXYZ(arr) => urdf_rs::Vec3(arr.map(|v| v.radians())),

Done in dfbf3d3.

mxgrey · 2023-07-31T16:36:54Z

rmf_site_format/src/workcell.rs

+            Geometry::Primitive(MeshPrimitive::Sphere { radius }) => urdf_rs::Geometry::Sphere {
+                radius: radius as f64,
+            },
+            _ => todo!("Only meshes and primitives are supported for conversion to urdf geometry"),


We should never merge code with the todo! macro in it (nor any form of panic! unless there's extremely strong justification). This indicates a code path that may contain unhandled errors. Also as mentioned in my other comment it will hide API changes that we want the compiler to catch for us.

If there's any conceivable situation where we might fail to convert a urdf_rs::Geometry into a Geometry then there is nothing at all wrong with foregoing the From<T> trait and writing a custom conversion function that returns a Result<T, E> where E is an enum that derives ThisError (see here for an example). This aligns with what Luca was saying about From<T>.

If you can write a correct From<Geometry> implementation without any potential panics then you're right that it's preferable to have From<T> instead of returning a Result<T, E>. But if we have to chose between From<T> with possible panics versus Result<T, E> without panics, then we absolutely must prefer Result<T, E>.

Gotcha, thank you for the clarification.

I made it so that we don't panic in 5e376ea.

mxgrey · 2023-07-31T16:41:25Z

rmf_site_format/src/workcell.rs

+
+                    let pose: urdf_rs::Pose = match frame.anchor {
+                        Anchor::Pose3D(pose) => pose.into(),
+                        _ => todo!(),


Let's make sure we implement this before we merge.

Done in dfbf3d3.

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

audrow · 2023-07-31T23:15:15Z

@mxgrey, thanks for the feedback!

I believe that I've implemented the changes. I'm fairly new to Rust (and loving it), so please let me know if you see opportunities for me to do things in a more Rust-like way.

luca-della-vedova

This is looking really great. High level design looks good to me, I went into a more detailed review

rmf_site_format/src/workcell.rs

rmf_site_editor/src/workcell/save.rs

luca-della-vedova · 2023-08-01T09:02:57Z

I also just noticed that the pose of links might be wrong without support for joints.
We have a hierarchy of Frames and reading a Frame's Pose will define its pose relative to its parent. The issue is that since links in URDF have no parent, if we naively set the link's pose to be the frame's pose, all the information about the hierarchy (i.e. the global transform of the parent) would be lost.

I believe a way to make this work would be to look at each link, its parent, then add a fixed joint between the two at export time, but I'm not sure if there are any other/better ways.

I'm OK with dealing with this in the next step of supporting joints but just thought I'd point that out so we keep it in mind.

audrow · 2023-08-01T13:34:00Z

@luca-della-vedova, thanks for the feedback - I feel like I'm learning a lot! I'll iterate on this a bit later this week.

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

audrow · 2023-08-03T20:46:56Z

I'm OK with dealing with this in the next step of supporting joints but just thought I'd point that out so we keep it in mind.

That's a good thing to point out. Let's handle it in another PR.

luca-della-vedova · 2023-08-07T02:48:19Z

rmf_site_format/src/workcell.rs

+                let parent = &f.1.parent;
+                let collision = parent_to_collisions
+                    .get(parent)
+                    .map(|collisions| collisions.clone())


I think the hierarchy calculation here might be off.
This line (and the same afterwards for visuals), do a lookup in the visual / collision hash map for the parent which is fine.
However, the parent variable doesn't contain the link's id but the link's parent id, so I believe this line would assign, to the current link, the visuals / collisions that are children of the link's parent link.
The parent should be the frame's ID f.0 rather than &f.1.parent, for readability can just unpack the tuple at the iteration level by changing:

.map(|f| {

Into

.map(|(frame_id, frame) {

With this change you should be able to also simplify the HashMap access to move values and remove the clone.
Since in every loop iteration the key you use to access the map is different, you can just move the visuals / collisions out of the map and avoid the cloning as such:

let visual = parent_to_visuals .remove(frame_id) .unwrap_or_default();

I noticed that hierarchy is also off in the example in your first post, the visual has a parent id of 0 that corresponds to the ID of the root of the workcell, while in the exported urdf its parent is set to the top_anchor frame which has id 2.
Now there is a minor challenge in the fact that the root might not be a frame itself but that should be pretty easy to fix by adding a dummy link at the origin

This should be fixed now in 6c5999c.

luca-della-vedova · 2023-08-07T02:58:02Z

Another small thing I noticed is that the root level tag is Robot instead of robot, however this should probably be fixed at the urdf-rs level. Did you try to import the exported urdf into any software to see whether parsers are case sensitive or not?

rmf_site_format/src/workcell.rs

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

audrow · 2023-08-18T23:17:13Z

Another small thing I noticed is that the root level tag is Robot instead of robot, however this should probably be fixed at the urdf-rs level. Did you try to import the exported urdf into any software to see whether parsers are case sensitive or not?

You were right. rviz2 needs lowercase. I added a string replace 6c5999c and made an issue on the urdf_rs repo.

audrow · 2023-08-18T23:19:37Z

I updated the urdf in the PR's description.

It still doesn't load into Rviz, but I think it will once we add joints.

I think that the visual and collisions are being included correctly now.

luca-della-vedova · 2023-08-21T02:27:47Z

Another small thing I noticed is that the root level tag is Robot instead of robot, however this should probably be fixed at the urdf-rs level. Did you try to import the exported urdf into any software to see whether parsers are case sensitive or not?

You were right. rviz2 needs lowercase. I added a string replace 6c5999c and made an issue on the urdf_rs repo.

It was a one-liner and they already fixed and re-released the crate, 0.7.3 contains the fix. I removed the workaround and set the minimum crate version to 0.7.3 in 9b7391c

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

luca-della-vedova

I think this is a good first step, we can tackle the remaining features in followups

audrow · 2023-08-21T22:18:05Z

@luca-della-vedova, great! I don't have merge access. Feel free to merge if you think it's ready.

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

audrow · 2023-09-15T14:07:13Z

Closing in favor of #177.

First try at workcell to URDF

116c451

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

audrow added 2 commits July 31, 2023 15:04

Add visual to urdf export

bb09860

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

Add collision

ccb7915

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

Merge branch 'main' into audrow/urdf-export

57e9592

audrow marked this pull request as ready for review July 31, 2023 15:43

mxgrey requested changes Jul 31, 2023

View reviewed changes

audrow added 2 commits July 31, 2023 21:16

Move convert Workcell to URDF int to_urdf

dfbf3d3

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

Don't panic on unknown pose type

5e376ea

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

audrow requested a review from mxgrey July 31, 2023 23:15

luca-della-vedova reviewed Aug 1, 2023

View reviewed changes

audrow added 4 commits August 3, 2023 19:18

Apply easy suggestions

c8ef823

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

Make pose simpler

77883bd

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

Use hashmap + remove unnecessary clones

41e04ff

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

Make saving approach consistent

cba2ebc

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

audrow requested a review from luca-della-vedova August 3, 2023 20:50

luca-della-vedova reviewed Aug 7, 2023

View reviewed changes

rmf_site_format/src/workcell.rs Outdated Show resolved Hide resolved

Make visual child of world frame

6c5999c

Signed-off-by: Audrow Nash <audrow@intrinsic.ai>

audrow force-pushed the audrow/urdf-export branch from e1ffff7 to 6c5999c Compare August 18, 2023 23:03

Merge branch 'main' into audrow/urdf-export

abce8ec

audrow requested a review from luca-della-vedova August 18, 2023 23:20

Remove workaround for upstream issue, minor cleanup

9b7391c

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

luca-della-vedova force-pushed the audrow/urdf-export branch from 0e22813 to 9b7391c Compare August 21, 2023 02:28

luca-della-vedova approved these changes Aug 21, 2023

View reviewed changes

Merge branch 'main' into audrow/urdf-export

328e12d

Remove last spurious clone

8ab7273

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

luca-della-vedova mentioned this pull request Aug 31, 2023

Urdf import, export and workcell editor fixes #177

Merged

audrow closed this Sep 15, 2023

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Save workcell as URDF #155

Save workcell as URDF #155

audrow commented Jul 27, 2023 •

edited

Loading

luca-della-vedova commented Jul 28, 2023

audrow commented Jul 31, 2023

audrow commented Jul 31, 2023

audrow commented Jul 31, 2023 •

edited

Loading

mxgrey left a comment

mxgrey Jul 31, 2023

audrow Jul 31, 2023

mxgrey Jul 31, 2023 •

edited

Loading

audrow Jul 31, 2023

audrow Jul 31, 2023

mxgrey Jul 31, 2023

audrow Jul 31, 2023

audrow commented Jul 31, 2023

luca-della-vedova left a comment

luca-della-vedova commented Aug 1, 2023

audrow commented Aug 1, 2023

audrow commented Aug 3, 2023

luca-della-vedova Aug 7, 2023

luca-della-vedova Aug 7, 2023

audrow Aug 18, 2023

luca-della-vedova commented Aug 7, 2023

audrow commented Aug 18, 2023 •

edited

Loading

audrow commented Aug 18, 2023

luca-della-vedova commented Aug 21, 2023 •

edited

Loading

luca-della-vedova left a comment

audrow commented Aug 21, 2023

audrow commented Sep 15, 2023

Save workcell as URDF #155

Save workcell as URDF #155

Conversation

audrow commented Jul 27, 2023 • edited Loading

luca-della-vedova commented Jul 28, 2023

audrow commented Jul 31, 2023

audrow commented Jul 31, 2023

audrow commented Jul 31, 2023 • edited Loading

mxgrey left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mxgrey Jul 31, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

audrow commented Jul 31, 2023

luca-della-vedova left a comment

Choose a reason for hiding this comment

luca-della-vedova commented Aug 1, 2023

audrow commented Aug 1, 2023

audrow commented Aug 3, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

luca-della-vedova commented Aug 7, 2023

audrow commented Aug 18, 2023 • edited Loading

audrow commented Aug 18, 2023

luca-della-vedova commented Aug 21, 2023 • edited Loading

luca-della-vedova left a comment

Choose a reason for hiding this comment

audrow commented Aug 21, 2023

audrow commented Sep 15, 2023

audrow commented Jul 27, 2023 •

edited

Loading

audrow commented Jul 31, 2023 •

edited

Loading

mxgrey Jul 31, 2023 •

edited

Loading

audrow commented Aug 18, 2023 •

edited

Loading

luca-della-vedova commented Aug 21, 2023 •

edited

Loading