Cable Robot Updates

[gchenfc]

This PR merges in updates to the cable robot code. I should have made this PR a long time ago and also split up changes into separate PR's, but I kept procrastinating.

Updates include:

• Winch dynamics - includes motor inertia/viscosity/friction which requires:
  • an additional factor to model the cable length acceleration
  • adding variables for cable acceleration and tension
• refactor cdpr_planar_controller to be a Base class that supports both iLQR and "naive" controllers
• support both Trapezoidal collocation and Forward-Euler, for dynamics integration
• drawing/plotting utilities
• scripts related to graffiti / GTGraffiti ICRA 2021 submission
  • gerry02 - gerry04
  • paint_parse
• initialization and Values copying utilities in utils.py
• "BlockEliminateSequential" does sequential elimination but able to eliminate multiple variables together, which simplifies the process of extracting the control law from the BayesNet.
  • Specifically, we want our control law to be like [torque1; torque2; torque3; torque4] = K * [pose ; twist] so we would like to group the torques (which are currently 4 scalar variables) together and the pose+twist together.
  • It is possible to obtain the control law without doing this, but just requires a lot more book-keeping in python and manually populating the K matrix row-by-row.

[dellaert]

Well, you have to add (one) reviewer. If you add me, I'll say: can you please split it up in a few, easy to review PRs?