Method for better-conditioned quaternion-based cost errors

bindings/pyc3/c3_systems_py.cc

@@ -175,10 +175,14 @@ PYBIND11_MODULE(systems, m) {
       .def_readwrite("lcs_factory_options",
                      &C3ControllerOptions::lcs_factory_options)
       .def_readwrite("state_prediction_joints",
-                     &C3ControllerOptions::state_prediction_joints);
+                     &C3ControllerOptions::state_prediction_joints)
+      .def_readwrite("quaternion_weight",
+                     &C3ControllerOptions::quaternion_weight)
+      .def_readwrite("quaternion_regularizer_fraction",
+                     &C3ControllerOptions::quaternion_regularizer_fraction);
 
   m.def("LoadC3ControllerOptions", &LoadC3ControllerOptions);
 }
 }  // namespace pyc3
 }  // namespace systems
-}  // namespace c3
+}  // namespace c3

core/BUILD.bazel

@@ -99,6 +99,7 @@ cc_test(
         "GUROBI_HOME",
         "GRB_LICENSE_FILE"
     ],
+    size = "small",
 )
 
 filegroup(

examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml

@@ -7,6 +7,10 @@ state_prediction_joints: []
   # - name : "CartSlider"
   #   max_acceleration : 10
 
+#leave empty to disable any quaternion-dependent cost weights
+quaternion_weight: 0.0
+quaternion_regularizer_fraction: 0.0
+
 lcs_factory_options:
   #options are 'stewart_and_trinkle' or 'anitescu'
   # contact_model : 'stewart_and_trinkle'

examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml

@@ -5,6 +5,10 @@ solve_time_filter_alpha: 0.0
 publish_frequency: 100
 state_prediction_joints: []
 
+#leave empty to disable any quaternion-dependent cost weights
+quaternion_weight: 0.0
+quaternion_regularizer_fraction: 0.0
+
 lcs_factory_options:
   #options are 'stewart_and_trinkle' or 'anitescu'
   # contact_model : 'stewart_and_trinkle'

multibody/BUILD.bazel

@@ -56,6 +56,7 @@ cc_test(
         ":lcs_factory",
         "@gtest//:main",
     ],
+    size = "small",
 )
 
 filegroup(

systems/BUILD.bazel

@@ -43,6 +43,7 @@ cc_library( name = "c3_controller",
         "//core:c3",
         "//core:options",
         "//multibody:lcs_factory",
+        "//systems:quaternion_error_hessian",
         "@drake//:drake_shared_library",
     ],
 )
@@ -90,6 +91,18 @@ cc_library(
 )
 
 
+cc_library(
+    name = "quaternion_error_hessian",
+    srcs = [
+        "common/quaternion_error_hessian.cc"
+    ],
+    hdrs = [
+        "common/quaternion_error_hessian.h"
+    ],
+    deps = [
+        "@drake//:drake_shared_library",
+    ],
+)
 
 cc_library(
     name = "system_utils",
@@ -117,6 +130,7 @@ cc_test(
         "GUROBI_HOME",
         "GRB_LICENSE_FILE"
     ],
+    size = "small",
 )
 
 cc_test(
@@ -129,6 +143,25 @@ cc_test(
         ":framework",
         "@gtest//:main",
     ],
+    size = "small",
+)
+
+cc_test(
+    name = "quaternion_cost_test",
+    srcs = [
+        "test/quaternion_cost_test.cc"
+    ],
+    data = [
+        "test/resources/cube.sdf",
+        "test/resources/plate.sdf",
+        "test/resources/quaternion_cost_test.yaml",
+    ],
+    deps = [
+        "//core:c3",
+        ":systems",
+        "@gtest//:main",
+    ],
+    size = "small",
 )
 
 filegroup(

systems/c3_controller.cc

@@ -4,6 +4,7 @@
 
 #include <Eigen/Dense>
 
+#include "common/quaternion_error_hessian.h"
 #include "core/c3_miqp.h"
 #include "core/c3_qp.h"
 #include "multibody/lcs_factory.h"
@@ -158,6 +159,9 @@ drake::systems::EventStatus C3Controller::ComputePlan(
   if (!state_prediction_joints_.empty())
     ResolvePredictedState(x0, filtered_solve_time);
 
+  // Update cost matrices with quaternion cost blocks
+  UpdateQuaternionCosts(x0, desired_state.value());
+
   // Update LCS and target in the C3 problem
   c3_->UpdateLCS(lcs);
   c3_->UpdateTarget(target);
@@ -272,5 +276,66 @@ void C3Controller::OutputC3Intermediates(
   }
 }
 
+void C3Controller::UpdateQuaternionCosts(const Eigen::VectorXd& x_curr,
+                                         const Eigen::VectorXd& x_des) const {
+  // Extract quaternion indices
+  vector<int> quaternion_indices;
+  for (const auto& body_idx : plant_.GetFloatingBaseBodies()) {
+    const auto& body = plant_.get_body(body_idx);
+    int start = body.floating_positions_start();
+    quaternion_indices.push_back(start);
+  }
+
+  // Early return if no quaternions or cost parameters not set
+  if (quaternion_indices.size() == 0 ||
+      !controller_options_.quaternion_weight.has_value() ||
+      !controller_options_.quaternion_regularizer_fraction.has_value()) {
+    return;
+  }
+
+  C3::CostMatrices costs = c3_->GetCostMatrices();
+  vector<MatrixXd> Q = costs.Q;
+  vector<MatrixXd> R = costs.R;
+  vector<MatrixXd> G = costs.G;
+  vector<MatrixXd> U = costs.U;
+
+  for (int index : quaternion_indices) {
+    Eigen::VectorXd quat_curr_i = x_curr.segment(index, 4);
+    Eigen::VectorXd quat_des_i = x_des.segment(index, 4);
+
+    Eigen::MatrixXd quat_hessian_i =
+        common::hessian_of_squared_quaternion_angle_difference(quat_curr_i,
+                                                               quat_des_i);
+
+    // Regularize hessian so Q is always PSD
+    double min_eigenval = quat_hessian_i.eigenvalues().real().minCoeff();
+    Eigen::MatrixXd quat_regularizer_1 =
+        std::max(0.0, -min_eigenval) * Eigen::MatrixXd::Identity(4, 4);
+    Eigen::MatrixXd quat_regularizer_2 = quat_des_i * quat_des_i.transpose();
+
+    // Additional regularization term to help with numerical issues
+    Eigen::MatrixXd quat_regularizer_3 = 1e-8 * Eigen::MatrixXd::Identity(4, 4);
+
+    double quaternion_weight = controller_options_.quaternion_weight.value();
+    double quaternion_regularizer_fraction =
+        controller_options_.quaternion_regularizer_fraction.value();
+
+    // Replace quaternion blocks in Q
+    double discount_factor = 1;
+    for (int i = 0; i < N_ + 1; i++) {
+      Q[i].block(index, index, 4, 4) =
+          discount_factor * quaternion_weight *
+          (quat_hessian_i + quat_regularizer_1 +
+           quaternion_regularizer_fraction * quat_regularizer_2 +
+           quat_regularizer_3);
+      discount_factor *= controller_options_.c3_options.gamma;
+    }
+  }
+
+  // Set C3 cost matrices
+  C3::CostMatrices new_costs(Q, R, G, U);
+  c3_->UpdateCostMatrices(new_costs);
+}
+
 }  // namespace systems
 }  // namespace c3

systems/c3_controller.h

@@ -71,6 +71,21 @@ class C3Controller : public drake::systems::LeafSystem<double> {
     c3_->UpdateCostMatrices(costs);
   }
 
+  /**
+   * @brief Returns the cost matrices used by the controller.
+   * @return The cost matrices to being used in the optimization problem.
+   */
+  const C3::CostMatrices& GetCostMatrices() { return c3_->GetCostMatrices(); }
+
+  /**
+   * @brief Updates any 4x4 portions of the cost weight matrix corresponding to
+   * quaternion states that are set to have quaternion-dependent costs.
+   * @param x_curr The current state vector.
+   * @param x_des The desired state vector.
+   */
+  void UpdateQuaternionCosts(const VectorXd& x_curr,
+                             const Eigen::VectorXd& x_des) const;
+
   /**
    * @brief Adds a linear constraint to the controller.
    * @param A The constraint matrix.
@@ -214,10 +229,11 @@ class C3Controller : public drake::systems::LeafSystem<double> {
   std::vector<JointDescription> state_prediction_joints_;
 
   // Cost matrices for optimization.
-  std::vector<Eigen::MatrixXd> Q_;  ///< State cost matrices.
-  std::vector<Eigen::MatrixXd> R_;  ///< Input cost matrices.
-  std::vector<Eigen::MatrixXd> G_;  ///< State-input cross-term matrices.
-  std::vector<Eigen::MatrixXd> U_;  ///< Constraint matrices.
+  mutable std::vector<Eigen::MatrixXd> Q_;  ///< State cost matrices.
+  mutable std::vector<Eigen::MatrixXd> R_;  ///< Input cost matrices.
+  mutable std::vector<Eigen::MatrixXd>
+      G_;  ///< State-input cross-term matrices.
+  mutable std::vector<Eigen::MatrixXd> U_;  ///< Constraint matrices.
 
   int N_;  ///< Horizon length.
 };

systems/c3_controller_options.h

@@ -60,6 +60,11 @@ struct C3ControllerOptions {
   double solve_time_filter_alpha = 0.0;
   double publish_frequency = 100.0;  // Hz
 
+  std::optional<double> quaternion_weight =
+      0.0;  // Quaternion cost scaling term
+  std::optional<double> quaternion_regularizer_fraction =
+      0.0;  // Outer product regularization scaling term
+
   std::vector<C3StatePredictionJoint> state_prediction_joints;
 
   template <typename Archive>
@@ -70,6 +75,8 @@ struct C3ControllerOptions {
     a->Visit(DRAKE_NVP(solve_time_filter_alpha));
     a->Visit(DRAKE_NVP(publish_frequency));
     a->Visit(DRAKE_NVP(state_prediction_joints));
+    a->Visit(DRAKE_NVP(quaternion_weight));
+    a->Visit(DRAKE_NVP(quaternion_regularizer_fraction));
 
     if (projection_type == "QP") {
       DRAKE_DEMAND(lcs_factory_options.contact_model == "anitescu");