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