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.

[Meow404]

Let me know if you want to discuss something online? I'll not be in the lab over spring break.

We had discussed the possibility of taking c3 objects directly, Was this not fitting this construct well?

@Meow404 reviewed 4 files and all commit messages, and made 13 comments.
Reviewable status: 4 of 9 files reviewed, 12 unresolved discussions (waiting on ebianchi).

---

core/c3.h line 59 at r1 (raw file):

    std::vector<Eigen::MatrixXd> G;
    std::vector<Eigen::MatrixXd> U;
    std::vector<Eigen::MatrixXd> Q_evaluation;

Do you still need this?

---

core/c3.cc line 431 at r1 (raw file):

  }
  vector<VectorXd> delta(N_, delta_init);
  vector<VectorXd> w(N_, VectorXd::Zero(n_z_));

Thank you for taking care of the namespaces.

---

core/traj_eval.h line 47 at r3 (raw file):

    int n_data = data[0].size();

any reason why the logic is kept in the header file?

---

core/traj_eval.h line 68 at r3 (raw file):

      const std::vector<Eigen::VectorXd>& data,
      const std::vector<Eigen::VectorXd>& data_des,
      const Eigen::MatrixXd& cost_matrix, Rest&&... rest) {

Isn't there a verson of this where the data_des is a single Eigen::VectorXd instead of an std::vector<Eigen::VectorXd>? Like x_des in c3.

---

core/traj_eval.h line 73 at r3 (raw file):

        std::forward<Rest>(rest)...);
  }
  /** Special case: no trajectory to evaluate. */

This can be a private/protected function

---

core/traj_eval.h line 81 at r3 (raw file):

   * feedforward control), and return the resulting trajectory of actual states
   * and inputs.
   *

Im wondering if a method to generate a fine_lcs from a coarse_lcs may be helpful. From the push_anything_dev maybe the ability to up-sample the LCS? We could leave G and U empty and potentially allow users to just send a single lcs. , It also would allow them to reuse the function if needed.

---

core/traj_eval.h line 104 at r3 (raw file):

                           const std::vector<Eigen::VectorXd>& u_plan,
                           const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd,
                           const LCS& coarse_lcs, const LCS& fine_lcs);

Why do we need the coarse_lcs? From the logic used in the file it seems the essential variable is timestep_split and fine_lcs. I think it's a bit contextual, but what about having a function that taken in timestep_split and is called by this function? (that way if I know my timestep_split, i dont have to regenerate an LCS)

---

core/traj_eval.cc line 75 at r3 (raw file):

  // Zero-order hold the planned trajectory to match the finer time
  // discretization of the LCS.
  std::pair<vector<VectorXd>, vector<VectorXd>> fine_plans =

does auto [x_plan_finer, u_plan_finer] = ZeroOrderHoldTrajectories(... work here? same for the others.

---

core/traj_eval.cc line 97 at r3 (raw file):

vector<VectorXd> TrajectoryEvaluator::SimulateLCSOverTrajectory(
    const VectorXd& x_init, const vector<VectorXd>& u_plan, const LCS& lcs) {

i think you should pass a variable of type LCSSimulateConfig, refer to lcs.h

---

core/traj_eval.cc line 114 at r3 (raw file):

    const vector<VectorXd>& x_plan, const vector<VectorXd>& u_plan,
    const LCS& lcs) {
  return SimulateLCSOverTrajectory(x_plan[0], u_plan, lcs);

Im looking at

  // Use the C3 plan.
  else if (cost_type == C3CostComputationType::kUseC3Plan) {
    for (int i = 0; i < N_ * resolution; i++) {
      UU[i] = z_fin[i / resolution].segment(n_x_ + n_lambda_, n_u_);
      XX[i] = z_fin[i / resolution].segment(0, n_x_);
      if (i == N_ - 1) {
        XX[i + 1] = lcs_for_cost.Simulate(XX[i], UU[i], simulate_config);
      }
    }
  }

from the push_anything_dev branch, what about simulating one step for every point of the x trajectory to create a new trajectory?

---

core/traj_eval.cc line 164 at r3 (raw file):

vector<VectorXd> TrajectoryEvaluator::DownsampleTrajectory(
    const vector<VectorXd>& fine_traj, const int& timestep_split) {

is upsample_rate more verbose instead of timestep_split?

---

core/traj_eval.cc line 198 at r3 (raw file):

  int timestep_split = fine_lcs.N() / coarse_lcs.N();
  DRAKE_THROW_UNLESS(fine_lcs.dt() * timestep_split == coarse_lcs.dt());
  DRAKE_THROW_UNLESS(coarse_lcs.N() * timestep_split == fine_lcs.N());

are the last two checks redundant? or is it to make sure fine_lcs.N() is divisible by coarse_lcs.N()