Method for better-conditioned quaternion-based cost errors

[ebianchi]

Starting from the source code from the repo, this PR brings over the method for better-conditioned quaternion-based cost errors. This relies on computing the 4-by-4 Hessian of the squared angle error between two quaternions, representing current and desired orientations, with respect to the current quaternion components.

Majority of logic taken directly from implementation in dairlib/push_anything, with the following changes

• Under quaternion_error_hessian.cc: when the current/desired quaternions are almost equal, the function returns a NaN matrix. Added a fallback equal to the closed-form limit of the hessian for when the inputs are very close (have only confirmed this is the limit numerically)
• In C3Controller::UpdateQuaternionCosts, added a small regularization term (1e-8 * identity matrix) to help numerical stability. Sometimes the minimum eigenvalue of the modified Q block is on the order of 1e-12, and this change is meant to prevent that from happening

Added 3 new parameters under c3_controller_options.h

• quaternion_indices: List of indices corresponding to the start of each quaternion block in the state vector. Can be empty if there are no quaternion states
• quaternion_weight: Scalar penalty term for all quaternion costs
• quaternion_regularizer_fraction: Scalar term for an outer product regularization term (set to 0 in sampling_c3)

Unit tests can be run via bazel test --test_output=all //systems:quaternion_cost_test

Still to do before review:

• Include regularization process necessary for preventing negative definite matrices (required for using it as a cost weight).
• Include unit tests with instructions on how to run.
• Provide guidance on how to include in an example (describe the role of new hyperparameters, identifying which turn on/off the feature and which tune the behavior).

---

This change is 

[github-actions]

LCOV of commit f67d1a0 during C3 Coverage #45

Summary coverage rate:
  lines......: 91.2% (1319 of 1446 lines)
  functions..: 75.2% (112 of 149 functions)
  branches...: 49.9% (960 of 1922 branches)

Files changed coverage rate:
                                            |Lines       |Functions  |Branches    
  Filename                                  |Rate     Num|Rate    Num|Rate     Num
  ================================================================================
  systems/c3_controller.cc                  | 4.8%    124|2967%     6|    -      0
  systems/c3_controller.h                   |50.0%     10| 0.0%     5|    -      0
  systems/c3_controller_options.h           |12.5%     16|1800%     1|    -      0
  systems/common/quaternion_error_hessian.cc| 1.4%    141| 600%     2|    -      0

[Meow404]

Thank you for your work. Some design comments, let me know if you would like to F2F discuss anything.

@Meow404 reviewed 13 of 13 files at r1, all commit messages.
Reviewable status: all files reviewed, 8 unresolved discussions (waiting on @ebianchi)

---

systems/test/quaternion_cost_test.cc line 74 at r1 (raw file):

             quat_regularizer_3);
        discount_factor *= controller_options.c3_options.gamma;
      }

This seems irregular. Simply because, if the function in c3_controller is updated in the future, the function itself is not tested and this test will continue to pass.

---

systems/test/quaternion_cost_test.cc line 87 at r1 (raw file):

  MatrixXd hessian =
      common::hessian_of_squared_quaternion_angle_difference(q1, q1);
  std::cout << hessian << std::endl;

Should this (std::cout) be removed?

---

systems/c3_controller.cc line 287 at r1 (raw file):

  G_.clear();
  U_.clear();

you can add an early return in the case no indices have been specified

---

systems/c3_controller.cc line 288 at r1 (raw file):

  U_.clear();

  double discount_factor = 1;

Could we do the following here :

• Check if Quaternion costs need to updated, if not return early
• Get the current costs from C3 using GetCostMatrices
• [Quaternion logic]
• Update the costs in C3 using UpdateCostMatrices

My intuition tells me that this logic can be used for all quaternion targets. So maybe should the default behavior should be to parse the joints and apply this logic to any joint types that may use quaternion values rather than using state_indices?

---

systems/c3_controller_options.h line 63 at r1 (raw file):

  double publish_frequency = 100.0;  // Hz

  std::vector<int>

Is it possible to avoid using indices? I find it challenging to keep track, and it's even harder for others to read your configuration file and understand what's going on.

An alternative, like done for state prediction is to use joint/state names.

---

systems/c3_controller_options.h line 65 at r1 (raw file):

  std::vector<int>
      quaternion_indices;          // Start indices for each quaternion block
  double quaternion_weight = 0.0;  // Quaternion cost scaling term

If these options are not essential for c3 to work, I would declare them as optional and and handle them later if they are defined. It allows users with already defined config files to continue using their files as is.

---

systems/c3_controller.h line 230 at r1 (raw file):

  mutable std::vector<Eigen::MatrixXd>
      G_;  ///< State-input cross-term matrices.
  mutable std::vector<Eigen::MatrixXd> U_;  ///< Constraint matrices.

This seems to be a mistake from my side since these variables were not originally used in the controller.

---

systems/common/quaternion_error_hessian.h line 20 at r1 (raw file):

 * @param quat_desired The second quaternion (size 4 vector).
 * @return The 4x4 Hessian matrix of the squared angle difference.
 */

Maybe mention the format of the quaternion as well. I'm still not sure if (w, x, y, z) or (x, y, z, w) is convention.