RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.36k stars 1.27k forks source link

[framework] Publish events at (tₑ, x⁺(tₑ)) instead of (tₑ, x⁻(tₑ)) #20256

Open jwnimmer-tri opened 1 year ago

jwnimmer-tri commented 1 year ago

Is your feature request related to a problem? Please describe.

Refer to the Simulator overview documentation for background reading.

Currently, publishes events always use (tₑ, x⁻(tₑ)) as their Context. I have a case where things get a lot simpler for me if I can publish at (tₑ, x⁺(tₑ)) instead. In short, I have a system that declares a concurrent timed unrestricted update event and timed publish event, and I'd like to publish the post-update state at tₑ, instead of at some unknown future point after the world has evolved tₑ+ω. (More specifically, I'd like a ZOH update event to capture a RgbdSensor output camera image, and a simultaneous publish event to publish that image during the "same" step.)

Note of course that selecting x⁺ vs x⁻ only applies to publish event declarations -- update events wouldn't/shouldn't/couldn't offer this.

Anyone coming from a background without continuous state (e.g., most RL researchers) will be extremely surprised about our current default (and only) publish event timing. Anyone making logs or traces from a Simulator today where they record the timestamp + data of their publish events (e.g., with VectorLogSink) will be surprised to find that their logs are all off-by-one in the time versus the published data.

Describe the solution you'd like

When declaring a publish event, the user should be able to declare that it publishes using x⁺ not x⁻. I would love it if that was our new default (I'd argue it's the least surprising choice), but for a while it will need to be opt-in so that we don't break any existing simulations.

Describe alternatives you've considered

There is really no other way to accomplish this goal in a robust and composable way. The x⁺ vs x⁻ is a fundamental modeling choice.

Additional context

Discussed in detail on drakedevelopers slack.

jwnimmer-tri commented 12 months ago

I studied this a bit more.

(1) Tentatively, I think only timed publish events need to distinguish x⁻ vs x⁺. So for witness-triggered publish, initialize-triggered publish, and force-triggered publish, there are no changes.

(2) For timed publish events, I think my proposal is to add a flag to choose pre-update, post-update, or both. Something like the following outline:

diff --git a/systems/framework/event.h b/systems/framework/event.h
index 2a908539d0..621d6cf32b 100644
--- a/systems/framework/event.h
+++ b/systems/framework/event.h
@@ -401,6 +401,12 @@ enum class TriggerType {
  */
 using TriggerTypeSet = std::unordered_set<TriggerType, DefaultHash>;

+enum class HybridPublish {
+  kPreUpdate = 1,
+  kPostUpdate = 2,
+  kPreAndPostUpdate = 3,
+};
+
 /** Abstract base class that represents an event. The base event contains two
 main pieces of information: an enum trigger type and an optional attribute
 that can be used to explain why the event is triggered.
diff --git a/systems/framework/leaf_system.h b/systems/framework/leaf_system.h
index c4a27a369f..d63ee429d4 100644
--- a/systems/framework/leaf_system.h
+++ b/systems/framework/leaf_system.h
@@ -281,7 +281,8 @@ class LeafSystem : public System<T> {
   template <class MySystem>
   void DeclarePeriodicPublishEvent(
       double period_sec, double offset_sec,
-      EventStatus (MySystem::*publish)(const Context<T>&) const) {
+      EventStatus (MySystem::*publish)(const Context<T>&,
+      HybridPublish hybrid_publish = HybridPublish::kPreUpdate) const) {
     static_assert(std::is_base_of_v<LeafSystem<T>, MySystem>,
                   "Expected to be invoked from a LeafSystem-derived System.");
     DRAKE_DEMAND(publish != nullptr);
@@ -307,7 +308,8 @@ class LeafSystem : public System<T> {
   template <class MySystem>
   void DeclarePeriodicPublishEvent(double period_sec, double offset_sec,
                                    void (MySystem::*publish)(const Context<T>&)
-                                       const) {
+                                       const,
+      HybridPublish hybrid_publish = HybridPublish::kPreUpdate) {
     static_assert(std::is_base_of_v<LeafSystem<T>, MySystem>,
                   "Expected to be invoked from a LeafSystem-derived System.");
     DRAKE_DEMAND(publish != nullptr);
@@ -587,7 +589,8 @@ class LeafSystem : public System<T> {
   @see Simulator::set_publish_every_time_step() */
   template <class MySystem>
   void DeclarePerStepPublishEvent(
-      EventStatus (MySystem::*publish)(const Context<T>&) const) {
+      EventStatus (MySystem::*publish)(const Context<T>&) const,
+      HybridPublish hybrid_publish = HybridPublish::kPreUpdate) {
     static_assert(std::is_base_of_v<LeafSystem<T>, MySystem>,
                   "Expected to be invoked from a LeafSystem-derived System.");
     DRAKE_DEMAND(publish != nullptr);

@sherm1 and I to discuss this week.

sherm1 commented 12 months ago

Maybe "PublishTiming" instead of "HybridPublish" for the enum?