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