Drake: Implement PID controller

[amcastro-tri]

Implement a PID controller based on the schematic below.

[hongkai-dai]

Is this a PI controller?

[amcastro-tri]

Previous to #3188 the original idea was to implement something like so:

class PidController : public Diagram<T> {
 public:
  explicit PidController(const T& Kp, const T& Ki, int length) {
    // ... Do all the wiring between the member primitives calling 
    //Diagram<T> methods like Connect and ExportInput/ExportOutput
  }

 private:
  std::unique_ptr<Adder<T>> adder_;
  std::unique_ptr<Integrator<T>> integrator_;
  std::unique_ptr<PassThrough<T>> pass_through_;
  std::unique_ptr<Gain<T>> proportional_gain_;
  std::unique_ptr<Gain<T>> integrator_gain_;
};

notice that with this solution the PidController is the owner of its sub-systems.

After #3188 (DiagramBuilder) the solution would in theory (does not work) like so:

Diagram<T> MakePidController(const T& Kp, const T& Ki, int length) {
  DiagramBuilder<T> builder;

  auto pass_through = make_unique<PassThrough>();
  auto proportional_gain = make_unique<Gain<T>>(Kp /* gain */, length /* length */);
  // ... and so on ...

  builder.ExportInput(pass_through->get_input_port(0));

  builder.Connect(pass_through->get_output_port(0),
                  proportional_gain->get_input_port(0));
  // ... and so on ....
  builder.Connect(integrator->get_input_port(0),
                  adder->get_input_port(1));

  builder.ExportOutput(adder->get_output_port(0));

  return builder.Build();
}

However, what happens with the individual sub-systems?

@david-german-tri and @sherm1?

[david-german-tri]

Great points. I think I've got an idea for how to handle this. Stay tuned for PR.

_Update: #3216_

[sherm1]

@hongkai-dai asked Is this a PI controller?

Meaning where is the derivative D term I presume. For our use that would likely be another input I think (i.e. if controlling position we would pass in position error and velocity error). Is that right @amcastro-tri?

[amcastro-tri]

Sorry, I missed @hongkai-dai question when posting the snippets above.
@hongkai-dai, you are correct the schematic you see above is representative of a PI controller. I just quickly drew that to show in #3132 the need for a PassThrough system which you can see in the schematic being used to connect the input to the PI controller to the inputs of each gain.

Meaning where is the derivative D term I presume. For our use that would likely be another input I think (i.e. if controlling position we would pass in position error and velocity error). Is that right

Yes, that is exactly right. I should update the schematic above.