Блог

Я пытаюсь понять, как использовать Drake для управления простым Двойным маятником с помощью ПИД-регулятора.

Следуя примеру по этой ссылке, я создал аналогичную программу, но я только хотел попробовать использовать 1 PID на нижнем соединении и посмотреть, что сделала система. (Я знаю, что таким образом невозможно перейти в стабильное состояние)

   MultibodyPlant<double>* dp = builder.AddSystem<MultibodyPlant<double>>(max_time_step);
  dp->set_name("plant");
  dp->RegisterAsSourceForSceneGraph(amp;scene_graph);

  Parser parser(dp);

  parser.AddModelFromFile(kDoublePendulumSdfPath);

  // Weld the base link to world frame with no rotation.
  const autoamp; root_link = dp->GetBodyByName("base");
  dp->AddJoint<WeldJoint>("weld_base", dp->world_body(), std::nullopt,
                          root_link, std::nullopt,
                          RigidTransform<double>::Identity());
  dp->AddJointActuator("a2", dp->GetJointByName("joint2"));

  // Now the plant is complete.
  dp->Finalize();

  // Create PID Controller.
  const Eigen::VectorXd Kp = Eigen::VectorXd::Ones(1) * Kp_;
  const Eigen::VectorXd Ki = Eigen::VectorXd::Ones(1) * Ki_;
  const Eigen::VectorXd Kd = Eigen::VectorXd::Ones(1) * Kd_;
  const auto* const pid = builder.AddSystem<PidController<double>>(Kp, Ki, Kd);
  builder.Connect(dp->get_state_output_port(),
                  pid->get_input_port_estimated_state());
  builder.Connect(pid->get_output_port_control(),
                  dp->get_actuation_input_port());
  //...
  

Это результат, который я получил, по-видимому, потому, что одно соединение не контролируется активно

 terminate called after throwing an instance of 'std::logic_error'
  what():  DiagramBuilder::Connect: Mismatched vector sizes while connecting output port continuous_state of System plant (size 4) to input port estimated_state of System drake/systems/controllers/PidController@00005639de6f78d0 (size 2)
Aborted (core dumped)
  

Поддерживает ли drake соединения, которые не контролируются активно, и как бы я это настроил?

Вместо построения ПИД-регулятора со скалярным коэффициентом усиления Kp, Kd, Ki, вы должны использовать вектор размера 2 в качестве коэффициента усиления. Поскольку вы хотите управлять только нижним соединением, то только запись, соответствующая нижнему соединению, не равна нулю в матрице усиления.

Я думаю, вы можете изменить свой код на

 const Eigen::VectorXd Kp = Eigen::Vector2d(0, 1) * Kp_;
const Eigen::VectorXd Ki = Eigen::Vector2d(0, 1) * Ki_;
const Eigen::VectorXd Kd = Eigen::Vector2d(0, 1) * Kd_;
const auto* const pid = builder.AddSystem<PidController<double>>(Kp, Ki, Kd);
  

где я предполагаю, что порядок состояния вашего объекта таков (upper_joint_position, lower_joint_position, upper_joint_velocity, lower_joint_velocity). Следовательно, коэффициент усиления равен [0, 1]. Если lower_joint_position предшествует upper_joint_position в вашем заводском состоянии, то коэффициент усиления должен быть [1, 0]