Real-To-Sim in Drake: What's The Proper Way to Get Real-World Sensor Data Into a Drake MultibodyPlant

Question

I've been reading through the Drake docs and any tutorials I found, but I've yet to see any detailed information about this problem/goal, so I thought I'd try to ask here.

Goal:
My current project is that I have a real-world furuta pendulum with sensors to obtain the cartpole equivalent of state vector q_sim = [x, theta, xdot, thetadot].
I want to be able have a real-world state-vector, q_real, that "overwrites" the simulated cartpole state-vector, q_sim, (acquired from MBP derived from Parser(plant=cart_pole).AddModelFromFile(sdf_path) ). So that the resulting behavior would be something like this: moving the real-world pendulum upright would be reflected (in real time) in the simulation pendulum moving upright. Eventually my goal would be able to have the furuta pendulum do a swing-up and balance behavior with the cartpole simulation reflecting the state vector, q_real, in real time.

What I've Thought About/Tried:

• Extending leafsystem to read the real-world sensors and return a state vector as if it was a modeled dynamical system, with its own actuator_port for the controller output. My problem with this approach was that I wasn't sure/know if it was possible to do this whilst still visualizing a cart_pole MBP with the same state vector. I came to this dead-end through my reading of the MultiBodyPlant doc [ https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html#details ]. I did not see any function to be able to replace/overwrite the simulation MBP's state_output_vector.
• I also considered the possibility of having two separate MBPs, one being the cart_pole derived from an .sdf file and another being an extended leafsystem that takes an actuator input, read sensors, and return a state vector output. However, with this setup I don't believe I am able to achieve the desired behavior of "moving the real-world pendulum upright and have the simulation pendulum move upright in real time"

Extra:
I also noticed that there's an Issue open on the drake github https://github.com/RobotLocomotion/drake/issues/12912about an official tutorial on drake's MBP from 2020. Does anyone know if there's any update to that tutorial beyond the Doxygen?

Answer 1

I don't think you want to overwrite your MBP plant with the data from simulation. The normal workflow would be to offer a different system that reads from your sensors and offers the same ports that your MBP would have offered.

In the extreme, you can even wrap all of your sensor and actuator drivers up into a system of their own that acts like a "mock" of the MBP. That's the pattern I offered in the ManipulationStation and ManipulationStationHardwareInterface. In addition to the doxygen, you could look here: https://manipulation.csail.mit.edu/robot.html#section4