Double pendulum with a single PID

Question

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?

score 2 · Accepted Answer · answered Sep 05 '20 at 18:10

2

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]

answered Sep 05 '20 at 18:10

Hongkai Dai

2,546
11
12

Would that mean I should add a join actuator over the ` dp->AddJointActuator("a1", dp->GetJointByName("upper_joint"));` as well? It seems counter intuitive to me to add a second PID given I'm not aiming to control that joint – cjds Sep 06 '20 at 02:32
Yes you need to add the joint actuator for the upper joint as well. This is because we assume pid controller has the same number of inputs as plant.num_positions(). – Hongkai Dai Sep 06 '20 at 04:10
I see. This seems like a different kind of question but in theory could I design a robot where an actuator is controlled by a PID and the other actuators are controlled by another controller? Is that possible within the framework of drake? – cjds Sep 06 '20 at 04:15
1

You could. If you want only lower joint to be controller by the PID controller, then I would add an output port to the plant to just expose the lower joint state (position/velocity), and then build a PID controller that just takes the lower joint state and output the lower joint torque. You could similarly add the other controller for the upper joint. – Hongkai Dai Sep 06 '20 at 04:18

Double pendulum with a single PID

1 Answers1