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)
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))
type=int,
help="Number of trials to run.",
default=5)
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run each sim.",
default=4.0)
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
plant = builder.AddSystem(QuadrotorPlant())
controller = builder.AddSystem(StabilizingLQRController(plant, [0, 0, 1]))
# returns y = Cx+Du+y0=-Kx+u0+Kx0=u0-K(x-x0)
x = np.zeros(12)
x[2] = 1
out = controller.D().dot(x) + controller.y0() # 1.22625
print(out)
import pdb
pdb.set_trace()
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 in MeshCat
scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterGeometry(scene_graph)
builder.Connect(plant.get_geometry_pose_output_port(),