双摆单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

不应使用标量 Kp、Kd、Ki 增益构建 PID 控制器，而应使用大小为 2 的矢量作为增益。既然只想控制下关节，那么增益矩阵中只有下关节对应的entry是non-zero

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

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]

双摆单PID

Double pendulum with a single PID

c++

drake