单 PID 双摆

Question

我正在尝试了解如何使用 Drake 使用 PID 控制器控制简单的双摆。

按照link 的示例，我创建了一个类似的程序，但我只想尝试在下关节上使用 1 个 PID，看看系统做了什么。 （我知道以这种方式不可能达到稳定状态）

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

  Parser parser(dp);

  parser.AddModelFromFile(kDoublePendulumSdfPath);

  // Weld the base link to world frame with no rotation.
  const auto& 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 是否支持不受主动控制的关节，我该如何设置？

Answer 1

您应该使用大小为 2 的向量作为增益，而不是构建具有标量 Kp、Kd、Ki 增益的 PID 控制器。既然只想控制下关节，那么增益矩阵中只有下关节对应的条目是非零的。

我认为您可以将代码更改为

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]