Add collocation slack forces DirconTrajectory object

[ShaneRozenLevy]

The goal of this modification is to make it easier to change the number of knot points when loading a trajectory and using it as the initial guess for another trajopt problem. By working in trajectories/splines you can re-interpolate to increase and decrease the number of knot points.

I add the collocation slack variables to the DirconTrajectory trajectory object while also adding functions which return a vector of trajectories for the contact forces, collocation forces, and collocation slack variables.

This code was initially developed for kod*lab's work on spirit, but it sounds like y'all might find it useful.

---

This change is 

[ShaneRozenLevy]

Looks great! Is the code used to reconstruct initial guesses for x, u, lambda etc given n knotpoints easy to turn into a PR. It might be worth adding if it's not too much work/if it fits well with this current set up because otherwise there don't see to be a clear use for reconstructing trajectories for the slack variables.

Reviewed 1 of 2 files at r1. Reviewable status: 1 of 2 files reviewed, 1 unresolved discussion (waiting on @ShaneRozenLevy)

_lcm/dircon_saved_trajectory.h, line 52 at r1 (raw file):_

  drake::trajectories::PiecewisePolynomial<double> ReconstructStateTrajectory()
      const;
  std::vector<drake::trajectories::PiecewisePolynomial<double>> ReconstructLambdaTrajectory()

Because the force and slack variables are not true trajectories, it may be clearer if there's comment explaining the purposes of these methods. ie. to make linear interpolation easier.

Looks great! Is the code used to reconstruct initial guesses for x, u, lambda etc given n knotpoints easy to turn into a PR. It might be worth adding if it's not too much work/if it fits well with this current set up because otherwise there don't see to be a clear use for reconstructing trajectories for the slack variables.

Reviewed 1 of 2 files at r1. Reviewable status: 1 of 2 files reviewed, 1 unresolved discussion (waiting on @ShaneRozenLevy)

_lcm/dircon_saved_trajectory.h, line 52 at r1 (raw file):_

  drake::trajectories::PiecewisePolynomial<double> ReconstructStateTrajectory()
      const;
  std::vector<drake::trajectories::PiecewisePolynomial<double>> ReconstructLambdaTrajectory()

Because the force and slack variables are not true trajectories, it may be clearer if there's comment explaining the purposes of these methods. ie. to make linear interpolation easier.

I think dircon already has that code. Here is how I currently set my initial guess:

    trajopt.drake::systems::trajectory_optimization::MultipleShooting::
        SetInitialTrajectory(u_traj, x_traj);
    for (int j = 0; j < sequence.num_modes(); j++) {
      trajopt.SetInitialForceTrajectory(j, l_traj[j], lc_traj[j],
                                        vc_traj[j]);
    }

Where u_traj, x_traj, l_traj, lc_traj, and vc_traj are all directly returned from the DirconTrajectory object.

[yangwill]

Merging this into master