feat(c3) : Add ability to evaluate cost of C3 plan

[ebianchi]

Addresses #31

---

This change is 

[github-actions]

LCOV of commit 21f98bb during C3 Coverage #95

Summary coverage rate:
  lines......: 92.5% (1751 of 1893 lines)
  functions..: 79.0% (154 of 195 functions)
  branches...: 53.2% (1376 of 2586 branches)

Files changed coverage rate:
                                            |Lines       |Functions  |Branches    
  Filename                                  |Rate     Num|Rate    Num|Rate     Num
  ================================================================================
  core/c3.cc                                | 8.0%    274|2162%    21|    -      0
  core/c3.h                                 |19.0%     21|1333%     3|    -      0
  core/lcs.cc                               |13.7%     51|1167%     6|    -      0
  core/lcs.h                                |17.6%     17|2200%     3|    -      0
  core/traj_eval.cc                         |14.7%     95|1382%    11|    -      0
  core/traj_eval.h                          |53.3%     15| 225%     8|    -      0

[Meow404]

The overall structure really looks good to me 🔥 . I haven't taken a deep dive, but having some nested namespaces for trajectory evaluation would make sure they are well scoped. Letting the functions be declared static as part of a class is also an option but not required.

[ebianchi]

This PR is ready for review. One remaining issue is the noble check failed due to:

OMP: Info #276: omp_set_nested routine deprecated, please use omp_set_max_active_levels instead.
Error code = 10030
Too many sessions, 5 active sessions for a baseline of 2
terminate called after throwing an instance of 'std::runtime_error'
  what():  Gurobi optimization failed.

@Meow404 I don't believe this is an issue because of the new code in this PR, but please advise if there's something to be done.

[Meow404]

@ebianchi A few minor points before I review:

• remove code earmarked TODO @bibit: remove
• Can the comments be in a similar structure found in other parts of the code eg.

  /**
   * @brief Computes the C3 solution and intermediates given a discrete state.
   * @param context The system context.
   * @param discrete_state The discrete state (usually the current state of the
   * system).
   * @return The event status after computation.
   */

• We discussed using default values or/and a varadic template style approach. What's done currently is quite alright. I just wanted to check if you had any issues or concerns regarding those approaches, and maybe track those concerns here if possible.

[ebianchi]

Thanks for pointing out the outdated comments and how to match formatting to the rest of the code. Those are done.

As for the variadic template style approach for the ComputeQuadraticTrajectoryCost function, it could reduce the redundant lines of code that sum cost terms, but it would duplicate some size checking lines (checking the compatibility of the lengths of the vectors x, u, and lambda to be of lengths N+1, N, and N, respectively). I drafted the variadic template style approach and found it to be more verbose than this alternative. If you think there's something I'm missing here that would make us reconsider the variadic approach, I'm happy to take another look.

[ebianchi]

After some further consideration, we decided it may be desirable to sum costs contributed by state, input, and/or contact force trajectories that don't match lengths N+1, N, N, respectively. Thus, users can give any arbitrary number of data_vecs, desired_data_vecs, cost_matrices trios to contribute to a quadratic trajectory cost summation. This is a more flexible implementation and could use variadic templating.