def get_LQR_params(self):
quadroter = QuadrotorPlant()
context = quadroter.CreateDefaultContext()
hover_thrust = self.m * self.g
quadroter.get_input_port(0).FixValue(context, [
hover_thrust / 4.0,
] * 4)
context.get_mutable_continuous_state_vector()\
.SetFromVector(self.nominal_state)
linear_sys = Linearize(quadroter, context, InputPortIndex(0),
OutputPortIndex(0))
return LinearQuadraticRegulator(linear_sys.A(), linear_sys.B(), self.Q,
self.R)
from pydrake.systems.framework import DiagramBuilder
from pydrake.systems.analysis import Simulator
from pydrake.examples.pendulum import PendulumPlant
from pydrake.examples.quadrotor import QuadrotorPlant
from pydrake.systems.controllers import PidController
from pydrake.systems.framework import GenerateHtml
from pydrake.systems.primitives import LogOutput
from pydrake.math import *
Quadrotor2D = Quadrotor2D_[None] # Default instantiation
N = 200
# max_dt = 0.5
# max_tf = N * max_dt
plant = QuadrotorPlant()
print(plant)
context = plant.CreateDefaultContext()
print(context)
# params = context.get_numeric_parameter(0)
dircol = DirectCollocation(plant,
context,
num_time_samples=N,
minimum_timestep=0.01,
maximum_timestep=0.05,
assume_non_continuous_states_are_fixed=True)
dircol.AddEqualTimeIntervalsConstraints()
thrust_limit = 10 # N*m.
u = dircol.input()
print(u)
x0 = dircol.initial_state()