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()