0

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.

Lars Erik Fagernaes
  • 182
  • 1
  • 12

2 Answers2

2

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
Russ Tedrake
  • 4,703
  • 1
  • 7
  • 10
0

I think we should be able to help, but probably need a few more details.

My mental model of your setup is that you use QuadrotorPlant to design the trajectory and the MakeFiniteHorizonLinearQuadraticRegulator methods in drake... the result of that is a drake System that has an input port expecting a vector of doubles representing the state of the quadrotor, and it outputs a vector of doubles that represent the thrust commands for the quadrotor.

Separately, you have a Gazebo simulator with a quadrotor, that is accepts commands and simulates the dynamics.

Are you trying to connect them via message passing (as opposed to a single executable)? Presumably with ROS1 messages? (If yes, then we have little systems in drake that can send/receive ROS messages, which we can point you to)

FWIW, I would definitely recommend trying the workflow with just running an LQR controller in drake before trying the trajectory version.

Russ Tedrake
  • 4,703
  • 1
  • 7
  • 10
  • So I actually define the equations of motion of the Quadrotor in a separate class and then add the dynamics as constraints to the mathematicalProgram as residuals. I never could couple the QuadrotorPlant to the MakeFiniteHorizonLinearQuadraticRegulator in one system, because when I instance MakeFiniteHorizonLinearQuadraticRegulator, it needs a Context. This context comes from QuadrotorPlant and demands that QuadrotorPlant has defined an input port. I figured this input port should be the output port of the LinearQuadraticRegulator controller, so we have this chicken and egg problem. – Lars Erik Fagernaes May 07 '20 at 21:18
  • Currently running the setup as a single executable, but flexible as to what interface to use. – Lars Erik Fagernaes May 07 '20 at 21:21
  • Regarding the `MakeFiniteHorizonLinearQuadraticRegulator` context... this is just like the way we pass a context into the `LinearQuadraticRegulator`. There should not be any chicken and egg problem...? Perhaps this example will help you? https://github.com/RobotLocomotion/drake/blob/bc4638164b9994119f8b50b48d268c172e34bdfe/examples/acrobot/test/run_swing_up_traj_optimization.cc#L83 – Russ Tedrake May 08 '20 at 02:12
  • Great, yes now it works. The error in my logic was thinking u0 from FiniteHorizonLinearQuadraticRegulatorOptions had to be "coupled" with the system, but it just represents an initial starting point.. Should have read the finite_horizon_linear_quadratic_regulator.cpp more thorough.. thanks!:) – Lars Erik Fagernaes May 08 '20 at 03:52