My objective is to use finite horizon linear quadratic regulator(FHLQR) to follow a trajectory generated by a mathematical program and simulate it in Gazebo. As FHLQR needs a system as input, I'm using the QuadrotorPlant, but I'm not sure if this is ideal. My problems arise when I try to connect the state from Gazebo into Drake. In short, what would be the proper way of coupling state from Gazebo with a controller such as FHLQR?
I've thought about editing the context of QuadrotorPlant to mirror the state in Gazebo, but after this update I'm having trouble getting control output from the controller. I've also thought about coupling the Simulator between the output of the controller and input of the QuadrotorPlant, but haven't figured out how to modify the Simulator to mirror Gazebo.
And for your gazebo interface, and assuming you're all in c++, then I'd imagine it will look something like this:
// setup
auto regulator = MakeFiniteHorizonLinearQuadraticRegulator(...);
auto context = regulator.CreateDefaultContext();
// during execution
context->SetTime(time_from_gazebo);
context->FixInputPort(0, Eigen::VectorXd([state obtained from gazebo]));
Eigen::VectorXd u = regulator->get_output_port(0)->Eval(context);
// then apply u to your gazebo interface