c++ - 双摆带单PID

Question

我试图了解如何使用 Drake 使用 PID Controller 来控制简单的双摆。
遵循此示例 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)

德雷克是否支持不受主动控制的关节，我将如何设置？

Answer 1

您应该使用大小为 2 的 vector 作为增益，而不是使用标量 Kp、Kd、Ki 增益构建 PID Controller 。既然你只想控制下关节，那么增益矩阵中只有下关节对应的条目是非零的。
我认为您可以将代码更改为

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]