sdformat
sdformat copied to clipboard
Should support SDFormat to URDF conversion
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 https://github.com/osrf/sdformat/issues/265, e.g. making sure compatibility checks are good, exposing that as public API.
\cc @IanTheEngineer
This is related to https://github.com/ros/urdf/issues/34 - Likely a tool that accomplishes this SDFormat -> URDF task could live side-by-side with the sdformat_parser
plugin for urdf::Model
's
In https://github.com/ros/sdformat_urdf/pull/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
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.
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.
EDIT 2: See Derp comment below: https://github.com/osrf/sdformat/issues/273#issuecomment-663271668
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.
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.
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!!! :facepalm:
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?
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.
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 astd::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, callsdf::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
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. PerhapsIsExpressibleAsDirectedTree()
?
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.
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.
- 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
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 :(
I'm not sure if I know why you're suggesting
sdf::addNestedModel
? Is it to constrain the query to a "root" model (b/csdf::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.