CODEWITHSUNDEEP
c++drakegazebo-simu
Lars Erik Fagernaes
Lars Erik Fagernaes

Reputation: 182

Using gazebo with drake for quadrotor

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.

Upvotes: 0

Views: 396

Answers (2)

Russ Tedrake
Russ Tedrake

Reputation: 5533

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

Upvotes: 2

Russ Tedrake
Russ Tedrake

Reputation: 5533

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.

Upvotes: 0

Related Questions