Double pendulum with a single PID

user_1059273 0
Viewed 115

I'm trying to understand how to use Drake to control a simple Double Pendulum using a PID controller.

Following the example at this link, I created a similar program but I only wanted to try to use 1 PID on the lower joint and see what the system did. (I am aware it is impossible to get to a stable state in this manner)

  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());
  //...

This is the output I got presumably because one joint is not actively controlled

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)

Does drake support joints that are not actively controlled, and how would I set that up?

drake c++
1 Answers

Instead of constructing PID controller with a scalar Kp, Kd, Ki gain, you should use a size 2 vector as the gain. Since you only want to control the lower joint, then only the entry corresponding to the lower joint is non-zero in the gain matrix.

I think you can change your code to

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);

where I assume the order of your plant state is (upper_joint_position, lower_joint_position, upper_joint_velocity, lower_joint_velocity). Hence the gain is [0, 1]. If lower_joint_position comes before upper_joint_position in your plant state, then the gain should be [1, 0]

user_19738610
Related