Should support SDFormat to URDF conversion

[EricCousineau-TRI]

Should ensure that SDFormat models are compatible (e.g. no loops, etc.).
URIs are up for debate for how to handle / convert (if at all).

Can help with #265, e.g. making sure compatibility checks are good, exposing that as public API.

\cc @IanTheEngineer

[IanTheEngineer]

This is related to ros/urdf#34 - Likely a tool that accomplishes this SDFormat -> URDF task could live side-by-side with the sdformat_parser plugin for urdf::Model's

[scpeters]

In ros/sdformat_urdf#1, there is an approach to parsing an sdformat file and generating urdfdom data structures. I've noticed one place where some additional APIs could be provided by libsdformat for finding the root link, since they are currently assuming that the canonical link is the root link (https://github.com/ros/sdformat_urdf/pull/1/files#diff-12b109e5d429371a684ac656c3aa2037R111-R126), which is not guaranteed to be true.

There was some prototype code for generating a KinematicGraph that could be used for finding the root link of a model, but it didn't make it into libsdformat9:

• https://github.com/osrf/sdformat/blob/spec2_prototype_9/include/sdf/FrameSemantics.hh#L68-L82
• https://github.com/osrf/sdformat/blob/spec2_prototype_9/src/FrameSemantics.cc#L167-L234

I think we could potentially add APIs like std::string Model::GetKinematicRootLink(const std::string &_linkNameGuess) and/or std::string Link::GetKinematicRootLink() that would use the KinematicGraph internally

[EricCousineau-TRI]

Per f2f, my opinion is that things like root link are effective implementation details of KDL. If it needs to be specified through SDFormat, it's non-physical (a modelilng consideration) and should possibly be a custom tag.

[scpeters]

Per f2f, my opinion is that things like root link are effective implementation details of KDL. If it needs to be specified through SDFormat, it's non-physical (a modelilng consideration) and should possibly be a custom tag.

Consider a directed graph with a vertex for each Link and an edge for each joint pointing from the resolved parent link to the resolved child link (aka directed kinematic graph). The direction of edges is a modeling choice and somewhat arbitrary, but if you ignore the directionality, the underlying undirected graph is still very physically meaningful. Would you be more comfortable if the libsdformat API provided API's that inform about the undirected kinematic graph:

• how many connected components are in this undirected graph?
• does this undirected graph have any cycles / loops?

Also, even if directionality is somewhat arbitrary, I still think it would be useful to query:

• is a fully-connected directed kinematic graph also a directed tree?
• if so, what is the root of that tree?

I don't think these questions are unique to KDL; they've been relevant to our previous work with dartsim and simbody, for example.

[EricCousineau-TRI]

EDIT 2: See Derp comment below: #273 (comment)

---

I get that these thing are relevant for libraries that model things as trees (Drake included), but I'm still feel like it's a bit of scope creep to add too many tree-based queries.

But yes, undirected graph queries sound great!

Regarding the directed parts:

• is a fully-connected directed kinematic graph also a directed tree?
• if so, what is the root of that tree?

IMO, it feels like the undirected queries can help inform this (connected components == 1, no cycles), and that's all libsdformat should be responsible for.
However, directed trees are not unique for a compatible graph, and I feel like that interpretation should be up to the implementing library depending on how they want to do internal optimization (but possibly with a heavier abstraction layer) or keep things convenient for the user (uber simple to implement).
The scope creep here is adding a feature that may require wayyyy too many knobs to be useful in a general sense.

Thoughts?

To clarify: I don't think libsdformat should foist the selection of a root node on users. They should do that themselves.
If they want something optimal w.r.t. algorithms they use, that's on them, and they can use whatever library to help get the best spanning tree for a graph?

EDIT 1: Follow-up is, if all downstream libraries use the exact same canonical representation of a tree for a given graph, then I'd be down for it. But if we have to add knobs, then I'd say nah. That, or libsdformat pulls in an external graph analysis lib, but I still feel like that's overkill for this library, and leaks in too many modeling details.

[EricCousineau-TRI]

Ooohhh, one more thing: Since URDF has //joint/@type=="floating", is there really a reason to constrain connected components == 1? I figured if you have disconnected components, then downstream libs can just say "meh, choose this root.", and then add floating bodies.

[EricCousineau-TRI]

Derp. This issue is about libsdformat providing SDFormat -> URDF conversion. Yeah, duh, sorry, we do need to choose a root link or expose that knob - my bad!!! 🤦

However, there is a question about what level of API libsdformat exposes in terms of facilitating those queries externally. I'm on the fence of whether or not we want a public commitment there, or just seal it into the API.

Thoughts?

[scpeters]

there is a question about what level of API libsdformat exposes in terms of facilitating those queries externally. I'm on the fence of whether or not we want a public commitment there, or just seal it into the API.

Yeah, we've hidden the FrameAttachedToGraph and PoseRelativeToGraph in libsdformat9, so I think for consistency we would do the same thing with a KinematicGraph and just expose something like the current Resolve* APIs. I'll think about this and make some concrete suggestions.

[scpeters]

how many connected components are in this undirected kinematic graph?

• std::set<std::string> Model::ConnectedLinks(const std::string &_linkName = ""): for a link name in a given model (default to canonical link if unspecified), return the set link names connected by joints within that model (this becomes more complicated once we allow joints across model boundaries with :: syntax). the output data structure could alternatively be a std::pair<std::set<sdf::Link*>, std::set<sdf::Joint*>>if you want DOM pointers to both Links and Joints
• std::vector<std::set<sdf::Link*>> Model::ConnectedLinkGroups(): returns all connected components

does this undirected graph have any cycles / loops?

• bool Model::HasKinematicLoop(), bool Link::ConnectedToKinematicLoop()

is a fully-connected directed kinematic graph also a directed tree? if so, what is the root of that tree?

• sdf::Errors Link::GetKinematicRootLink(std::string &_rootLinkName): starting with the given link:
  • if this link is not a child of any joints: return this link's name
  • if this link is a child of 2 or more joints: return an error
  • if this link is a child of 1 joint: repeat this loop for the parent link of that joint (we'll to also need to keep a list of visited links to avoid getting stuck in a loop)
• bool Model::IsDirectedTree(std::string &_rootLinkName, const std::set<std::string> &_connectedLinks) to check if a directed graph is also a directed tree, call sdf::Errors Link::GetKinematicRootLink(std::string &_rootLinkName) for every link in a set of connected links and confirm that there are no errors and that they all return the same rootLinkName

[EricCousineau-TRI]

That looks better!

Some minor questions / comments:

• I'm not sure how I feel about these queries living on sdf::Model, esp. if there may be connections outside of that model (e.g. in a parent). I would think that URDF could express "multi-model"s as trees, even if there is a floating joint between them. Is it possible to put them somewhere else, and perhaps constraint them to operate only on the root-level model? (or is there a benefit to this structure that I'm missing?)
• The name GetKinematicRootLink seems a bit dubious / could be better named? e.g. GetCandidateKinematicRootForLink? (that way it's explicitly clear that it's not returning the "one true" root?)
• IsDirectedTree - I'm not sure if I understand this... How does (string rootLink, set<string> connectedLinks) indicate direction? Is it based on the (parent, child) relationship that dictates that direction? If so, I'm not sure if we want to explicitly state this. Perhaps IsExpressibleAsDirectedTree()?

[traversaro]

Should ensure that SDFormat models are compatible (e.g. no loops, etc.).

"Compatible" means that the frame placement must respect the existing URDF (both C++ DOM and XML format) constraints on the position of the link frame? The main constraint is that for 1-dof joints the link frame origin must lie on the axis of the parent joint. If that is the case, in practice there in most models there is only one possible choice of kinematic root for the model that respects this constraint.

[scpeters]

Should ensure that SDFormat models are compatible (e.g. no loops, etc.).

"Compatible" means that the frame placement must respect the existing URDF (both C++ DOM and XML format) constraints on the position of the link frame? The main constraint is that for 1-dof joints the link frame origin must lie on the axis of the parent joint. If that is the case, in practice there in most models there is only one possible choice of kinematic root for the model that respects this constraint.

that's a good point @traversaro; I hadn't thought about that additional difference, that URDF link frames are co-located with the parent joint frame. We should make a test for this.

I think this can be handled by computing the values of //joint/origin, //collision/origin, and //visual/origin relative to the parent joint pose instead of the parent link pose.

[scpeters]

• I'm not sure how I feel about these queries living on sdf::Model, esp. if there may be connections outside of that model (e.g. in a parent). I would think that URDF could express "multi-model"s as trees, even if there is a floating joint between them. Is it possible to put them somewhere else, and perhaps constraint them to operate only on the root-level model? (or is there a benefit to this structure that I'm missing?)

I think it would take some careful thought for how to represent nested models in urdf DOM data structures. Would you use the approach of sdf::addNestedModel from libsdformat9 that concatenates nested model names with link names using :: as a separator? I know we are moving away from that in libsdformat11, but that may be needed to support nested models here.

• The name GetKinematicRootLink seems a bit dubious / could be better named? e.g. GetCandidateKinematicRootForLink? (that way it's explicitly clear that it's not returning the "one true" root?)

That's a fine suggestion; it is more specific at the cost of being longer.

• IsDirectedTree - I'm not sure if I understand this... How does (string rootLink, set connectedLinks) indicate direction? Is it based on the (parent, child) relationship that dictates that direction? If so, I'm not sure if we want to explicitly state this. Perhaps IsExpressibleAsDirectedTree()?

Since it is a method of the Model class, the directionality should already be available internally. Passing a set of connected link names allows subsets of a model to be evaluated for whether they can be expressed as a directed tree. I'm open to name suggestions here too, but on first glance IsExpressibleAsDirectedTree strikes me as overly verbose

[EricCousineau-TRI]

I think it would take some careful thought [...]

I'm not sure if I know why you're suggesting sdf::addNestedModel?
Is it to constrain the query to a "root" model (b/c sdf::Model is the root model by construction?)
Or is it permit flattening to express it in URDF?

I don't think sdf::addNestedModel is good for either of those purposes, and if URDF were to express nested models, it should leverage the DOM API once we have it.

That being said, it also doesn't seem like we need to rush this in? Shane already has a working prototype, so we could wait til we have proper nested models and only the solve the problem there?

That's a fine suggestion; it is more specific at the cost of being longer.

GetPossibleRootLink() or GetPlausibleRootLink? I'd rather the truthiness be preserved, and the (semi-redundant) specifier ("kinematic") be lost.

[...] IsExpressibleAsDirectedTree strikes me as overly verbose

CanBeADirectedTree? IsDirectedTreeCompatible?

I mean, these probably aren't going to be used that much, so meh on length? And also, they're not that long. I've seen longer :P -- and for much more frequently used functions :(

[scpeters]

I'm not sure if I know why you're suggesting sdf::addNestedModel?
Is it to constrain the query to a "root" model (b/c sdf::Model is the root model by construction?)
Or is it permit flattening to express it in URDF?

I don't think sdf::addNestedModel is good for either of those purposes, and if URDF were to express nested models, it should leverage the DOM API once we have it.

I didn't mean to suggest we should literally use sdf::addNestedModel, merely that we should use the same approach that it uses to flatten the nested model namespace.

That being said, it also doesn't seem like we need to rush this in?

Agreed that it doesn't need to be in the first implementation.