I first built a mass in SDF and a prismatic joint with and on the bottom, then I built it into a quadcopter using the propeller module in Drake. In order to control the drone to reach the target point, I used linear MPC to build the control system. Additionally, I followed other answer and set the controller’s output variable to a discrete variable and added a zero-order hold between the controller and the drone model. The solve time of each MPC program is between 10-20ms, and the update interval of the controller output variables is 0.1 seconds.

    quadrotor_plant, visualizer = create_quadrotor(quadrotor_builder, world_plant, scene_graph)
        world_builder.AddSystem(quadrotor_plant)
    
    # create MPC controller
    initial_thrust = [15.0, 15.0, 15.0, 15.0]
    time_step = 0.25
    mpc_controller = MpcController_Discrete(quadrotor_plant, initial_state, final_state, time_horizon, 
    num_time_samples, thrust_limit, initial_thrust, time_step)
    mpc_controller = world_builder.AddSystem(mpc_controller)
    # 添加 Zero Order Hold
    zoh = ZeroOrderHold(time_step, len(initial_thrust), 0.5)
    zoh = world_builder.AddSystem(zoh)

    # 连接系统
    world_builder.Connect(mpc_controller.get_output_port(0), zoh.get_input_port())
    world_builder.Connect(zoh.get_output_port(), quadrotor_plant.get_input_port(0))
    world_builder.Connect(quadrotor_plant.get_output_port(0), mpc_controller.get_input_port(0))

The strange thing is that the drone can be simulated smoothly when it is in the air, and the controller can run normally; but as soon as the bottom spring touches the ground, the simulation will get stuck, and the scheduled update of the controller will not work, usually for 15 seconds.

Some tests also found that even changing the joint type to fixed will cause the system to freeze due to contact with the ground. And what I can confirm is that the MPC system was not working during the stuck process, and the solution time did not change much. I am using pydrake to simulate. I want to find the cause of the struck, (maybe contact), and if there is any better design approach in Drake to avoid the possibility of interaction between the controller system and the Multibody Plant simulation.