mbp: Enable associating pure frames with SG frames

[EricCousineau-TRI]

At present, it seems like we only have a mapping from a plant's body to a FrameId; if we want to get an arbitrary frame's mapping to the scene graph, we have to do an awkwardly redundant offset dance: https://github.com/RobotLocomotion/drake/blob/53f44f47c4e399b7573bcbe23b028cf57082011b/examples/manipulation_station/manipulation_station.cc#L462-L472

It would be nice if a user to explicitly say that they want SG to handle a frame, thus we don't have to do the dance; instead, we could do plant.GetFrameIdIfItExists(frame). In the manip station case, the camera frames are not added to the plant, but in Anzu in other cases, we do have the camera frames added (and TBH, I very much like it that way).

@SeanCurtis-TRI Would adding something like MBP::RegisterSceneGraphFrame(const Frame&) be the right way to do this? (Would it be horrible for performance?)

[SeanCurtis-TRI]

To be perfectly frank, it took me several reads to appreciate what is being communicated here. I've erased my first babbling incomprehension in favor of this next pass.

Here's my best guess:

1. Right now you don't like the fact that you're declaring the camera frame C w.r.t. a bodyframe P.
2. The reason you don't like it is that there is ostensibly an additional frame already specified in the MBP (via an SDF, for example) at that same pose. This would be the source of the "redundancy".
3. If you could affix the camera to this alternative frame, the pose in that frame would be the identity.

Does that basically capture it? The rest of the text assumes that my inference is correct.

1. Currently, every frame registered with SceneGraph is assumed to be a dynamic frame -- that means it is not rigidly affixed to its parent frame.
2. Dynamic frames increase the size of the output of the plant and input of scenegraph -- every dynamic frame must be provided a pose at every output evaluation. If a frame is in fact rigidly affixed to its parent frame, this leads to redundant runtime cost.
3. Therefore, I conclude that registering an additional frame is not really what you want to do.

However, as I look at the code and re-read it, I grow progressively less sure of this response. So, I'd ask, can you provide a code snippet of what you'd want to see that API you'd like that has a corresponding effect?

[EricCousineau-TRI]

Yup, that captures it (sorry for the ambiguity!).

It would be nice to do something like the following:

 const optional<geometry::FrameId> frame_id = 
     plant_->GetFrameIdIfExists(info.parent_frame->index()); 
 DRAKE_THROW_UNLESS(frame_id.has_value()); 
 auto camera = 
     builder.template AddSystem<systems::sensors::dev::RgbdCamera>( 
         camera_name, frame_id.value(), info.properties, false); 

(removing offset to encourage people to add their frames during construction, since they will most likely want to add a camera in the physical model anywho.)

It's a very small change, but feels cleaner for reasons 1-3 that you cited above.

For response (2), it's not clear how expensive this would be, especially if, instead of registering bodies, frames were the main way to communicate between MBP and SG. I would think it could increase it by a factor of 4x, but it's just a pose bundle, and it's using cached kinematics when updating the pose?

If the computation / communication is expensive and we wish to save on it, it seems like SG (with non-trivial work, admittedly) could take a little more information on frame relationships (purely if it's rigidly fixed or not - nothing else) to save data.

[SeanCurtis-TRI]

This makes sense and I guess I have two thoughts:

1. The RgbdCamera (probably renamed to RgbdSensor by the time it comes out of dev) could take a constructor where the pose is default Identity. That alleviates part of the problem.
2. You and I had talked in the past that, ultimately, the MBP logic of only registering frames that it associates collision/visual geometries with is ultimately going to go away. To facilitate more generalized parsing, MBP should ultimately register every declared frame (which includes body frames and frames attached to body frames). Those values will be cached on the MBP output. If there's an SG hit, then we can communicate to SG that a particular frame happens to be rigidly affixed to a dynamic frame and please be smart about it.

Does that satisfy things?

[EricCousineau-TRI]

Yup, that'd be excellent!

I'd still like to advocate that we strip out all offsets, and always force users to add frames to the SG / MBP, but that can always happen after this.

[SeanCurtis-TRI]

The primary reason I can't subscribe to that philosophy is that I have to hew to the idea that SceneGraph has to serve more than MBP.

[EricCousineau-TRI]

For brief computations, like computing Jacobians with lots of offsets, I can understand a component not wanting to add permanent frames for those things (esp. if orientation has no meaning). However, for something that is permanently being added (a camera, with individual frames per image), it would be really nice to have that accessible from more than just the RgbdCamera output port / properties, without having to be too creative, and without having to excessively propagate those offsets and keep semantic track of them.

It's definitely not a huge requirement, but I have an inkling that it would simplify a lot of code. IMO, our code in Anzu is getting simplified because we've started using frames more, rather than an awkwardly redundant association of body + offset sprinkled throughout distinct locations in code / configuration.

To ground the discussion, is there a concrete non-MBP component that will be actively using SG that cannot add a frame to its scene graph when adding a camera for each of the frames it cares about?

[SeanCurtis-TRI]

You currently have the option of specifying a frame (with an Identity pose that could be sugared away). The current API doesn't preclude this. However, you're taking it a step further than I like by requiring a frame. Obviously, any geometry source can register frames. That capability is there. But the fact that one must introduce a frame to SG and preclude a non-identity pose for the camera seems to swing too far in the other direction. Ironically, it seems because the workflow you want isn't optimized, you're seeking to preclude alternate workflows.

The only way I could subscribe to this is if I were to introduce frames that were neither geometry frames (implied by geometries) nor dynamic frames. But, at that point, it's just a pose anyways. So, I am as yet unpersuaded on the real value there.

[EricCousineau-TRI]

From my perspective, the real value is (a) code simplicity / maintainability (fewer public overloads with offsets) and (b) promoting cleaner workflows (make sure what you're adding has meaningful semantics). But yeah, I wouldn't say it's necessary for this issue at this point.

[EricCousineau-TRI]

This came up again as a pain point (in Anzu).

That being said, I don't believe I will be able to address it. Unassigning myself, passing to @SeanCurtis-TRI for disposition (closing if you think it's not worth addressing).

[EricCousineau-TRI]

(To clarify, I'd be happy to relax the philosophy of "requiring only frames and stripping out offsets".)

[SeanCurtis-TRI]

BTW It seems this whole thing would be happily resolved with a constructor overload -- i.e., specify the parent frame and have X_PB be identity by implication.

You'd still be responsible through some other means to make sure the frame you're affixing the camera to exists and is updated appropriately. But the camera doesn't have to know anything about it.

And, if you have a "dynamic frame" that is actually rigidly affixed to another dynamic frame, such that the total number of poses is increased by that one frame...who cares? It's not like the number of camera frames is really going to dominate.

If you agree, I'll knock out a quick PR.

[jwnimmer-tri]

Possibly this is a duplicate of #15002. Or at least, they are related.

[SeanCurtis-TRI]

Two thoughts:

1. As @jwnimmer-tri pointed out above, #15002 would resolve this.
2. Furthermore, since the issue was originally created, CameraConfig has been introduced and mitigates this. The pose X_PB of the camera sensor can specify arbitrary frames. That arbitrary frame gets handled in the depths of the camera config functions (technically,in sim_rgbd_sensor.cc).

Generally, we should be pushing people towards using the config function for adding a camera. If they do this, the problems encountered in the code snippets evaporate completely.

For those reasons, I'm closing this issue. If anyone disagrees, feel free to re-open with the rebuttal.

[EricCousineau-TRI]

Thanks! Agreed this can be closed out as duplicate of Russ's issue. Agreed on "config for camera workflow". I still have reservations on a pure code workflow, but I think that is fully captured by Russ's issue.