Control a part of the MultibodyPlant

Question

I wang to use LQR to control a quadrotor in the MultibodyPlant, but I dont know how to control a part of the MultibodyPlant.

For example, the MakeMultipleQuadrotors() includes a sdf model which has a quadrotor and a payload. I just want to control the first twelve state and collect the other state.But in the code it will control all the state. I wonder how to modify my code or some examples for it. Thank you!

def MultipleQuadrotorDemo():
    builder = DiagramBuilder()
    robot_diagram = builder.AddSystem(MakeMultipleQuadrotors())
    plant = robot_diagram.plant()

    # Create the LQR controller
    robot_diagram_context = robot_diagram.CreateDefaultContext()
    mass = plant.CalcTotalMass(
        plant.GetMyContextFromRoot(robot_diagram_context),
        [plant.GetModelInstanceByName("quad")])
    gravity = plant.gravity_field().gravity_vector()[2]
    nominal_input = [-mass * gravity / 4] * 4
    robot_diagram.get_input_port().FixValue(robot_diagram_context,
                                            nominal_input)
    Q = np.diag((([10] * 12)))
    R = np.eye(4)
    print(robot_diagram)
    print(robot_diagram_context)
    controller = builder.AddSystem(LinearQuadraticRegulator(robot_diagram, robot_diagram_context, Q, R))
    builder.Connect(controller.get_output_port(),
                    robot_diagram.get_input_port())
    builder.Connect(robot_diagram.GetOutputPort("x"),
                    controller.get_input_port())

    MeshcatVisualizer.AddToBuilder(builder,
                                   robot_diagram.GetOutputPort("query"),
                                   meshcat)
    diagram = builder.Build()

    simulator = Simulator(diagram)
    simulator.set_target_realtime_rate(1.0 if running_as_notebook else 0.0)
    context = simulator.get_mutable_context()

score 0 · Answer 1 · answered Jul 11 '23 at 16:45

0

MakeMultipleQuadrotors is not a Drake function so I can only speculate here: it looks like MakeMultipleQuadrotors exports the actuation input port (for all actuated dofs) of the MultibodyPlant in the diagram. If you want to only control some of the actuated dofs instead of all of them, there's the per-model-instance actuation input port of MultibodyPlant that you could connect your controller output to.

answered Jul 11 '23 at 16:45

Xuchen Han

326
1
6

Thank you for your answer. I was wondering if I could use the LinearQuadraticRegulator function that comes with drake for control (the input variable context contains all the states of the system, can I extract a portion of the states in context for control), or some other trajectory-planning function to make one of the models follow a predetermined trajectory, capturing the the state of the other models. Also I want to know is it possible to extract the state from context through ROS2 topic? – Howard Li Jul 16 '23 at 07:40
It looks like you can't use the `LinearQuadraticRegulator` sugar that takes the context when you only want to control part of the state, but there are more generic signatures that you could use. I'm not familiar with ROS2. – Xuchen Han Jul 18 '23 at 20:22

Control a part of the MultibodyPlant

1 Answers1