Effort limits for KinematicTrajectoryOptimization

[RussTedrake]

While this is technically possible now, it certainly isn't easy.

In order to be parallel with AddPositionBounds, AddVelocityBounds, etc, and to leverage the fact that we can create one shared_pointer constraint for all of the times, my proposal would be to add the following method to KinematicTrajectoryOptimization:

  /** Adds generic (nonlinear) constraints to enforce the effort limits defined
  in the plant at a sequence of normalized times, `s`:
  @verbatim
  B u_min ≤ M(q)v̇ + C(q, v)v - τ_g(q) - τ_app ≤ B u_max
  @endverbatim
  where q, v, and v̇ are evaluated at s. B is the plant's actuation matrix,
  u_min is MultibodyPlant::GetEffortLowerLimits(), u_max is
  MultibodyPlant::GetEffortUpperLimits(), and M, C, τ_g, and τ_app are the
  plant's mass matrix, Coriolis force, gravity, and applied force,
  respectively.

  `plant` and `plant_context` must remain valid for the lifetime of these
  constraints.

  @pre plant.is_finalized()
  @pre plant.num_positions() == num_positions()
  @pre B u_min ≤ B u_max
  */
  std::vector<std::vector<solvers::Binding<solvers::Constraint>>>
  AddEffortBoundsAtNormalizedTimes(const MultibodyPlant<double>& plant,
                                   const Eigen::Ref<const Eigen::VectorXd>& s,
                                   systems::Context<double>* plant_context);

cc @cohnt, @hongkai-dai ... in case you have any suggestions.

[aelvio]

This would be awesome and would definitely cover 90% of my usage needs! Thank you for considering it!

If possible, it would be nice to add custom bounding box constraints (or costs) to the effort beyond what is written in the MultibodyPlant. For instance, I'm imagining using the default effort limits at all of the control points to constrain the overall trajectory, but then adding tighter constraints (or a cost) at a few key normalized times to ensure that the trajectory hits a "balanced banking angle" where centrifugal and gravitational forces are balanced.