Write measured sensor values to ECM for deterministic closed-loop control

[shameekganguly]

Desired behavior

When sensors like ForceTorque sensors and distance sensors are used for closed-loop control, it is important to have deterministic lag between the sensor values and the control command update. However in Gz Sim, sensor values must be read via Transport which does not have deterministic latency. So for example, it is possible that the last F/T sensor value received is from one or more sim time-steps ago when computing the control command.

Ideally, the measured sensor value can be read directly from the ECM following the PostUpdate call for the System that manages the sensor (e.g. ForceTorque system). This way, there is a guarantee of at most one time-step lag in reading sensor values when computing the control, assuming that the sensor is set to update at each sim cycle.

Implementation suggestion

Specifically for ForqueTorque sensor, we could add a new ForceTorqueMeasured component that stores the last updated F/T sensor msg and populate this in ForceTorque::PostUpdate. This would also require ForceTorqueSensor to expose the measured sensor value from the class interface. In future, similar components can be added for other sensors commonly used for closed-loop control.

[traversaro]

Related to https://github.com/gazebosim/gz-sim/issues/2268 .

[scpeters]

Ideally, the measured sensor value can be read directly from the ECM

Specifically for ForqueTorque sensor, we could add a new ForceTorqueMeasured component that stores the last updated F/T sensor msg and populate this in ForceTorque::PostUpdate.

I agree that writing to the ECM is more deterministic than publishing over gz-transport, but there are some trade-offs here:

• System::PostUpdate is parallelized into separate threads (see SimulationRunner.cc) with the intent of speeding up the real-time factor for systems with many sensors

the consequence of that parallelization is that:

• System::PostUpdate does not have write access to the ECM (see System.hh documentation). So it is not currently possible to write to the ECM during PostUpdate.

Related to https://github.com/gazebosim/gz-sim/issues/2268.

One option is to move sensor updates from PostUpdate to Update and specify that the sensor Update calls are executed after the physics Update using the prototype code in https://github.com/gazebosim/gz-sim/pull/2394 based on the approach discussed in #2268.

[scpeters]

I'm adding an API to access the most recent wrench measurement to the ForceTorqueSensor in https://github.com/gazebosim/gz-sensors/pull/449 (currently only accessible via gz-transport)

[scpeters]

I'm adding an API to access the most recent wrench measurement to the ForceTorqueSensor in gazebosim/gz-sensors#449 (currently only accessible via gz-transport)

I've also opened a gz-sim pull request to write the measured wrench to the ECM in the ForceTorque Update call, using the configurable execution order from #2487 to ensure that the ForceTorque::Update call is after the Physics::Update