def test_basics(self):
# call default constructor
QuadrotorPlant()
quadrotor = QuadrotorPlant(m_arg=1, L_arg=2, I_arg=np.eye(3),
kF_arg=1., kM_arg=1.)
self.assertEqual(quadrotor.m(), 1)
self.assertEqual(quadrotor.g(), 9.81)
StabilizingLQRController(quadrotor, np.zeros(3))
def test_basics(self):
# call default constructor
QuadrotorPlant()
quadrotor = QuadrotorPlant(m_arg=1,
L_arg=2,
I_arg=np.eye(3),
kF_arg=1.,
kM_arg=1.)
self.assertEqual(quadrotor.m(), 1)
self.assertEqual(quadrotor.g(), 9.81)
StabilizingLQRController(quadrotor, np.zeros(3))
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)
def test_geometry(self):
builder = DiagramBuilder()
quadrotor = builder.AddSystem(QuadrotorPlant())
scene_graph = builder.AddSystem(SceneGraph())
state_port = quadrotor.get_output_port(0)
geom = QuadrotorGeometry.AddToBuilder(builder, state_port, scene_graph)
self.assertTrue(geom.get_frame_id().is_valid())
def test_basics(self):
# call default constructor
QuadrotorPlant()
quadrotor = QuadrotorPlant(m_arg=1,
L_arg=2,
I_arg=np.eye(3),
kF_arg=1.,
kM_arg=1.)
self.assertEqual(quadrotor.m(), 1)
self.assertEqual(quadrotor.g(), 9.81)
self.assertEqual(quadrotor.length(), 2)
self.assertEqual(quadrotor.force_constant(), 1)
self.assertEqual(quadrotor.moment_constant(), 1.)
np.testing.assert_array_equal(quadrotor.inertia(), np.eye(3))
StabilizingLQRController(quadrotor, np.zeros(3))
def test_basics(self):
# call default constructor
QuadrotorPlant()
quadrotor = QuadrotorPlant(m_arg=1,
L_arg=2,
I_arg=np.eye(3),
kF_arg=1.,
kM_arg=1.)
self.assertEqual(quadrotor.m(), 1)
self.assertEqual(quadrotor.g(), 9.81)
scene_graph = SceneGraph()
quadrotor.RegisterGeometry(scene_graph)
self.assertTrue(quadrotor.source_id().is_valid())
quadrotor.get_geometry_pose_output_port()
StabilizingLQRController(quadrotor, np.zeros(3))
# y_traj = PPTrajectory(sample_times, num_vars, degree, continuity_degree)
# y_traj.add_constraint(t=0, derivative_order=0, lb=START_STATE[:3]) # initial position
# y_traj.add_constraint(t=0, derivative_order=1, lb=[0, 0, 0]) # initial velocity
# y_traj.add_constraint(t=0, derivative_order=2, lb=[0, 0, 0]) # initial acceleration
# y_traj.add_constraint(t=1, derivative_order=0, lb=[0, 1.0, 0])
# y_traj.add_constraint(t=2, derivative_order=0, lb=[0, 2.0, 0])
# y_traj.add_constraint(t=3, derivative_order=0, lb=[0, 3.0, 0])
# y_traj.add_constraint(t=tf, derivative_order=0, lb=[0, 4.0, 0]) # end at zero
# y_traj.add_constraint(t=tf, derivative_order=1, lb=[0, 0, 0])
# y_traj.add_constraint(t=tf, derivative_order=2, lb=[0, 0, 0])
# y_traj.generate()
# Build
builder = DiagramBuilder()
plant = builder.AddSystem(QuadrotorPlant())
controller = builder.AddSystem(QuadrotorController(plant, y_traj, DURATION))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
# Set up visualization and env in MeshCat
from pydrake.geometry import Box
from pydrake.common.eigen_geometry import Isometry3
from pydrake.all import AddMultibodyPlantSceneGraph
from pydrake.multibody.tree import SpatialInertia, UnitInertia
env_plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
for idx, object_ in enumerate(OBJECTS):
sz, trans, rot = object_
body = env_plant.AddRigidBody(
str(idx + 1),
def test_geometry(self):
builder = DiagramBuilder()
quadrotor = builder.AddSystem(QuadrotorPlant())
scene_graph = builder.AddSystem(SceneGraph())
state_port = quadrotor.get_output_port(0)
QuadrotorGeometry.AddToBuilder(builder, state_port, scene_graph)
from pydrake.examples import quadrotor
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()