Add UFO Example

[sloretz]

Resolves #182

This adds an example of controlling a flying object using RViz, Drake-ROS, and the Drake systems framework.

Compiler warnings pointed out that the mass properties of the translucent lookout weren't being used, so I added them. Using them offsets the mass a little so that forces applied at the body's origin case the saucer to wobble.

wobbly_ufo.webm

---

This change is 

[EricCousineau-TRI]

Sweet, thanks!
I've made some initial comments to see if we can shrink down the size of the file and some additional nits.
Is it OK with you if we make these improvements in this PR?

Reviewed 6 of 6 files at r1, all commit messages.
Reviewable status: all files reviewed, 12 unresolved discussions (waiting on @sloretz)

---

drake_ros_examples/README.md line 42 at r1 (raw file):

- [IIWA manipulator](./examples/iiwa_manipulator): an RViz visualization of a static IIWA arm.
- [Multiple robots](./examples/multirobot): an RViz visualization of an array of Kuka LBR iiwa manipulators.
- [UFO](./examples/multirobot): an RViz visualization of a flying object controlled with the Drake systems framework and commandedusing RViz.

nit Missing space between "command" and "using"

---

drake_ros_examples/README.md line 42 at r1 (raw file):

- [IIWA manipulator](./examples/iiwa_manipulator): an RViz visualization of a static IIWA arm.
- [Multiple robots](./examples/multirobot): an RViz visualization of an array of Kuka LBR iiwa manipulators.
- [UFO](./examples/multirobot): an RViz visualization of a flying object controlled with the Drake systems framework and commandedusing RViz.

nit Can you word-wrap this file to 80 character lines?

---

drake_ros_examples/examples/ufo/README.md line 5 at r1 (raw file):

## Overview

The UFO example shows how to use Drake-ROS and the Drake systems framework to

nit For consistency, should use either Drake ROS or drake-ros.

---

drake_ros_examples/examples/ufo/ufo.cc line 64 at r1 (raw file):

/// Adds body named FlyingSaucer to the multibody plant.
void AddFlyingSaucer(MultibodyPlantd* plant) {

Is it possible to move this into a SDFormat file?

---

drake_ros_examples/examples/ufo/ufo.cc line 146 at r1 (raw file):

}

class SplitRigidTransform : public LeafSystemd {

While I understand the desire for piping vectors around, I think it'd be best to instead just plumb around SpatialForced and RigidTransformd as Value<> instead.
I think that would then shrink the size of this file (along with porting the model to SDFormat)

See examples here, derived from Anzu:
https://github.com/EricCousineau-TRI/repro/blob/6048da3/drake_stuff/python_profiling/components_cc_py.cc
(context: https://stackoverflow.com/a/74201119/7829525 - ignore the codestyle, as it's meant to illustrate hacking / prototyping)

---

drake_ros_examples/examples/ufo/ufo.cc line 262 at r1 (raw file):

  //  output: Vector3d f_S_W
  auto* forces_pid_controller = builder.AddSystem<PidControllerd>(
      Vector3d{100.0f, 100.0f, 2500.0f}, Vector3d{0.0f, 0.0f, 50.0f},

nit Unclear why these gains were chosen.
Can you add comment, e.g. // Empirically chosen gains. May need tuning?

---

drake_ros_examples/examples/ufo/ufo.cc line 262 at r1 (raw file):

  //  output: Vector3d f_S_W
  auto* forces_pid_controller = builder.AddSystem<PidControllerd>(
      Vector3d{100.0f, 100.0f, 2500.0f}, Vector3d{0.0f, 0.0f, 50.0f},

nit Having a non-zero integrator gain implies that you're fighting gravity, i.e. not including a feedforward component in your controller.

Can you add a TODO, e.g. something like this?

// TODO(eric.cousineau): Add in feedforward component and remove need for integrator gains.

Or if you'd like to add this yourself, I can provide example (if you need it).

---

drake_ros_examples/examples/ufo/ufo.cc line 362 at r1 (raw file):

class RosPoseGlue : public LeafSystemd {
 public:
  explicit RosPoseGlue(double extra_z = 0.0) : extra_z_(extra_z) {

Hm... extra_z has an old smell to it.

Instead of this, can you instead make a separate, simpler RigidTransformPremultiplier?

---

drake_ros_examples/examples/ufo/ufo.cc line 376 at r1 (raw file):

 private:
  void CalcDrakeTransform(const Contextd& context,

These are redundant w.r.t geometry_conversions.

Can you use them instead?

---

drake_ros_examples/examples/ufo/ufo.cc line 427 at r1 (raw file):

                  ufo_controller->GetInputPort("X_WS"));

  // TODO(sloretz) replace when RobotLocomotion/drake#16923 is solved

Solved by yours truly ;)
RobotLocomotion/drake#18171

Though I'm not sure if that solves this problem? Or did you have another intent?

---

drake_ros_examples/examples/ufo/ufo.cc line 465 at r1 (raw file):

}

std::unique_ptr<Contextd> SetInitialConditions(Diagramd* diagram) {

Name of function and actual execution are not the same - this also creates a context.

Can you either rename the function, or change the function to mutate an existing context?

---

drake_ros_examples/examples/ufo/ufo.cc line 474 at r1 (raw file):

      diagram->GetMutableSubsystemContext(plant, diagram_context.get());

  RigidTransformd X_WS(RollPitchYawd(0.0, 0.0, 0.0), Vector3d(0.0, 0.0, 0.0));

BTW I think this is the default, no? If so, why not just remove?

[sloretz]

Reviewable status: 2 of 8 files reviewed, 6 unresolved discussions (waiting on @EricCousineau-TRI)

---

drake_ros_examples/README.md line 42 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit Missing space between "command" and "using"

8e4c1e7

---

drake_ros_examples/README.md line 42 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit Can you word-wrap this file to 80 character lines?

8e4c1e7

---

drake_ros_examples/examples/ufo/README.md line 5 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit For consistency, should use either Drake ROS or drake-ros.

0af4228

---

drake_ros_examples/examples/ufo/ufo.cc line 64 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

Is it possible to move this into a SDFormat file?

Moved to sdformat files in 728ef17

---

drake_ros_examples/examples/ufo/ufo.cc line 146 at r1 (raw file):

While I understand the desire for piping vectors around, I think it'd be best to instead just plumb around SpatialForced and RigidTransformd as Value<> instead.

I would love to use those types instead of vectors! I thought I was forced to use vectors if I wanted to reuse systems like PidControllerbecause that's what it's input/output port types are.

I see the example you linked has fewer, but bigger LeafSystems. There seems to be more normal looking C++ logic and less plumbing through the Drake systems framework. Is that what you'd like to see instead? That is, A SaucerController which is just one LeafSystem with gravity feedforward and PID code inside instead of a Diagram of smaller controllers doing the individual pieces of that?

---

drake_ros_examples/examples/ufo/ufo.cc line 262 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit Unclear why these gains were chosen.
Can you add comment, e.g. // Empirically chosen gains. May need tuning?

74c1a20

---

drake_ros_examples/examples/ufo/ufo.cc line 262 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit Having a non-zero integrator gain implies that you're fighting gravity, i.e. not including a feedforward component in your controller.

Can you add a TODO, e.g. something like this?

// TODO(eric.cousineau): Add in feedforward component and remove need for integrator gains.

Or if you'd like to add this yourself, I can provide example (if you need it).

Replaced I gain with gravity feedforward in cb1b6d1

---

drake_ros_examples/examples/ufo/ufo.cc line 465 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

Name of function and actual execution are not the same - this also creates a context.

Can you either rename the function, or change the function to mutate an existing context?

Renamed to CreateInitialConditions in d0b49e1

---

drake_ros_examples/examples/ufo/ufo.cc line 474 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

BTW I think this is the default, no? If so, why not just remove?

Removed in d0b49e1

[EricCousineau-TRI]

Reviewed 6 of 6 files at r2, all commit messages.
Reviewable status: all files reviewed, 8 unresolved discussions (waiting on @sloretz)

---

drake_ros_examples/examples/ufo/ufo.cc line 146 at r1 (raw file):

Previously, sloretz (Shane Loretz) wrote…

While I understand the desire for piping vectors around, I think it'd be best to instead just plumb around SpatialForced and RigidTransformd as Value<> instead.

I would love to use those types instead of vectors! I thought I was forced to use vectors if I wanted to reuse systems like PidControllerbecause that's what it's input/output port types are.

I see the example you linked has fewer, but bigger LeafSystems. There seems to be more normal looking C++ logic and less plumbing through the Drake systems framework. Is that what you'd like to see instead? That is, A SaucerController which is just one LeafSystem with gravity feedforward and PID code inside instead of a Diagram of smaller controllers doing the individual pieces of that?

Given that you're now doing PD control, not PID control, I think it could be easier.

Additionally, some of the code that I linked to is complex because it's computing forces against a frame, rather than body.
You can greatly simplify that code (I can help if you like!).

Additionally, using more expressive types means we can start to dog-food LeafSystem conversions at some point in the future.

---

drake_ros_examples/examples/ufo/ufo.cc line 60 at r2 (raw file):

ModelInstanceIndex AddFlyingSaucer(MultibodyPlantd* plant) {
  auto parser = Parser(plant);
  std::filesystem::path pkg_share_dir{

BTW Apologies for dumb question, but does ROS 2 not have a resource-retriever type affordance?
(or are you using index directly because you'd like the file path, and not just content stream?)

---

drake_ros_examples/examples/ufo/ufo.cc line 69 at r2 (raw file):

/// Adds Ground geometry to the world in the multibody plant.
void AddGround(MultibodyPlantd* plant) {

nit Rather than compose these scenes using code, can I ask that you make a ufo_scene.sdf that uses merge-include to compose the models?

\cc @azeey

---

drake_ros_examples/examples/ufo/ufo.cc line 423 at r2 (raw file):

}

std::unique_ptr<Contextd> CreateInitialConditions(Diagramd* diagram) {

nit Hm... This is a rather small function now.

Can I ask that you inline the code?

---

drake_ros_examples/examples/ufo/ufo.cc line 451 at r2 (raw file):

    CreateInitialConditions(diagram.get());

  RunSimulation(diagram.get(), std::move(diagram_context));

nit Ownership movement here is a bit confusing; I'd like to avoid having first-time users potentially seeing this.

Can I ask that you also inline the RunSimulation function for now?

[sloretz]

Reviewable status: 6 of 9 files reviewed, 4 unresolved discussions (waiting on @EricCousineau-TRI)

---

drake_ros_examples/examples/ufo/ufo.cc line 146 at r1 (raw file):

Additionally, some of the code that I linked to is complex because it's computing forces against a frame, rather than body.
You can greatly simplify that code (I can help if you like!).

Yes please. I'm not sure what it means to compute forces against a body, but I'd like to learn.

Used a single LeafSystem instead of reusing Drake's built in systems in 7f0d9fc . I'm not sure about how X_WS from the previous step is being stored. I copied what DiscreteDerivative seems to be doing, but I'm not sure what would happen if the state was updated faster than the input measurement and the force from the derivative term went to zero.

---

drake_ros_examples/examples/ufo/ufo.cc line 362 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

Hm... extra_z has an old smell to it.

Instead of this, can you instead make a separate, simpler RigidTransformPremultiplier?

Added RigidTransformPremultiplier in 8de9eb8

---

drake_ros_examples/examples/ufo/ufo.cc line 376 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

These are redundant w.r.t geometry_conversions.

Can you use them instead?

Using geometry_conversions in de58267

---

drake_ros_examples/examples/ufo/ufo.cc line 427 at r1 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

Solved by yours truly ;)
RobotLocomotion/drake#18171

Though I'm not sure if that solves this problem? Or did you have another intent?

Ah, yeah I see how that resolves that ticket. I think when I wrote that I wanted a short cut to go from the SpatialForce output from the controller to a list of ExternallyAppliedSpatialForce to be input into a multibody plant. IIUC ExternallyAppliedSpatialForceMultiplexer offers a way to concatenate vectors of ExternallyAppliedSpatialForce, but a system still has to construct a list of ExternallyAppliedSpatialForce in the first place. I replaced the TODO with a comment saying that's what the AppliedSpatialForceVector system is doing in c4799f1.

---

drake_ros_examples/examples/ufo/ufo.cc line 60 at r2 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

BTW Apologies for dumb question, but does ROS 2 not have a resource-retriever type affordance?
(or are you using index directly because you'd like the file path, and not just content stream?)

It does, and resource_retriever package:// URLs do pretty much exactly this. I forgot about it 😶.

In 6826486 I had to pass the package path into the PackageMap() anyways for the <include>'s to work, so I didn't make it use resource retriever.

---

drake_ros_examples/examples/ufo/ufo.cc line 69 at r2 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit Rather than compose these scenes using code, can I ask that you make a ufo_scene.sdf that uses merge-include to compose the models?

\cc @azeey

6826486

---

drake_ros_examples/examples/ufo/ufo.cc line 423 at r2 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit Hm... This is a rather small function now.

Can I ask that you inline the code?

Inlined in 81fac61

---

drake_ros_examples/examples/ufo/ufo.cc line 451 at r2 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit Ownership movement here is a bit confusing; I'd like to avoid having first-time users potentially seeing this.

Can I ask that you also inline the RunSimulation function for now?

Inlined in 81fac61

[EricCousineau-TRI]

Reneging from comment above, I'd say we could land this on `main` then do any further refinements off of `master`!

Reviewed 2 of 3 files at r3, all commit messages.
Reviewable status: 8 of 9 files reviewed, 1 unresolved discussion (waiting on @sloretz)

---

drake_ros_examples/examples/ufo/ufo.cc line 146 at r1 (raw file):

Previously, sloretz (Shane Loretz) wrote…

Additionally, some of the code that I linked to is complex because it's computing forces against a frame, rather than body.
You can greatly simplify that code (I can help if you like!).

Yes please. I'm not sure what it means to compute forces against a body, but I'd like to learn.

Used a single LeafSystem instead of reusing Drake's built in systems in 7f0d9fc . I'm not sure about how X_WS from the previous step is being stored. I copied what DiscreteDerivative seems to be doing, but I'm not sure what would happen if the state was updated faster than the input measurement and the force from the derivative term went to zero.

Thanks! For now, I'd say let's get this merged in. Then I can (hopefully) file a follow-up PR!

And to be clear, this is less about "don't reuse Drake systems", but rather "don't have too much systems-level indirection if we need to curate our control spaces etc."

---

drake_ros_examples/examples/ufo/ufo.cc line 427 at r1 (raw file):

Previously, sloretz (Shane Loretz) wrote…

Ah, yeah I see how that resolves that ticket. I think when I wrote that I wanted a short cut to go from the SpatialForce output from the controller to a list of ExternallyAppliedSpatialForce to be input into a multibody plant. IIUC ExternallyAppliedSpatialForceMultiplexer offers a way to concatenate vectors of ExternallyAppliedSpatialForce, but a system still has to construct a list of ExternallyAppliedSpatialForce in the first place. I replaced the TODO with a comment saying that's what the AppliedSpatialForceVector system is doing in c4799f1.

SGTM, thanks!

---

drake_ros_examples/examples/ufo/ufo.cc line 60 at r2 (raw file):

Previously, sloretz (Shane Loretz) wrote…

It does, and resource_retriever package:// URLs do pretty much exactly this. I forgot about it 😶.

In 6826486 I had to pass the package path into the PackageMap() anyways for the <include>'s to work, so I didn't make it use resource retriever.

OK Excellent! At some point, we should provide affordance to leverage ament index with Drake's package map - which I see you've mentioned below.

---

drake_ros_examples/examples/ufo/ufo.cc line 157 at r3 (raw file):

    // Orientation PD controller - proportional
    t_S_W +=
        (R_error.ToRollPitchYaw().vector().array() * kp_rot_.array()).matrix();

BTW I don't love RPY for error terms, but I don't have succinct mathematical reasoning yet.

---

drake_ros_examples/examples/ufo/ufo.cc line 312 at r3 (raw file):

  auto ufo_controller = builder.AddSystem<FlyingSaucerController>(
      saucer_mass, plant.gravity_field().gravity_vector(),
      Vector3d{100.0, 100.0, 100.0},  // kp & kd chosen by trial and error

nit This is a bit hard to read. Can you try using param struct? e.g.,
https://github.com/EricCousineau-TRI/repro/blob/3784a154a094a59538cf6ae150f8633ef9cdfcd7/drake_stuff/python_profiling/components_cc_py.cc#L207-L216

[sloretz]

Reviewable status: 7 of 9 files reviewed, all discussions resolved (waiting on @EricCousineau-TRI)

---

drake_ros_examples/examples/ufo/ufo.cc line 312 at r3 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit This is a bit hard to read. Can you try using param struct? e.g.,
https://github.com/EricCousineau-TRI/repro/blob/3784a154a094a59538cf6ae150f8633ef9cdfcd7/drake_stuff/python_profiling/components_cc_py.cc#L207-L216

Added param struct in 9c861c6

It looks like the linked example is using designated initializes, which is a C++ 20 feature. Drake seems to be using c++ 17: https://github.com/RobotLocomotion/drake/blob/4eaa4be3e8ab7ff07b4f5cd7267e6b2bdf4f3d81/tools/cc_toolchain/bazel.rc#L5-L6 . I used a temporary variable instead.

[sloretz]

Updated to main, but I'll hold off on bazelizing until #133 merges so this can make use of ament_index_share_files to access the world and models.

Reviewable status: 5 of 9 files reviewed, all discussions resolved (waiting on @EricCousineau-TRI)