Method for better-conditioned quaternion-based cost errors

bindings/pyc3/c3_systems_py.cc

@@ -175,10 +175,16 @@ 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_indices",
+                     &C3ControllerOptions::quaternion_indices)
+      .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,11 @@ state_prediction_joints: []
   # - name : "CartSlider"
   #   max_acceleration : 10
 
+quaternion_indices: []
+#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,11 @@ solve_time_filter_alpha: 0.0
 publish_frequency: 100
 state_prediction_joints: []
 
+quaternion_indices: []
+#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,22 @@ cc_test(
         ":framework",
         "@gtest//:main",
     ],
+    size = "small",
+)
+
+cc_test(
+    name = "quaternion_cost_test",
+    srcs = [
+        "test/quaternion_cost_test.cc"
+    ],
+    data = [
+        "test/quaternion_cost_test.yaml"
+    ],
+    deps = [
+        ":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,11 @@ 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());
+  C3::CostMatrices new_costs(Q_, R_, G_, U_);
+  c3_->UpdateCostMatrices(new_costs);
+
   // Update LCS and target in the C3 problem
   c3_->UpdateLCS(lcs);
   c3_->UpdateTarget(target);
@@ -272,5 +278,54 @@ void C3Controller::OutputC3Intermediates(
   }
 }
 
+void C3Controller::UpdateQuaternionCosts(const Eigen::VectorXd& x_curr,
+                                         const Eigen::VectorXd& x_des) const {
+  Q_.clear();
+  R_.clear();
+  G_.clear();
+  U_.clear();
+
+  double discount_factor = 1;
+  for (int i = 0; i < N_; i++) {
+    Q_.push_back(discount_factor * controller_options_.c3_options.Q);
+    discount_factor *= controller_options_.c3_options.gamma;
+    if (i < N_) {
+      R_.push_back(discount_factor * controller_options_.c3_options.R);
+      G_.push_back(controller_options_.c3_options.G);
+      U_.push_back(controller_options_.c3_options.U);
+    }
+  }
+  Q_.push_back(discount_factor * controller_options_.c3_options.Q);
+
+  for (int index : controller_options_.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 discount_factor = 1;
+    for (int i = 0; i < N_ + 1; i++) {
+      Q_[i].block(index, index, 4, 4) =
+          discount_factor * controller_options_.quaternion_weight *
+          (quat_hessian_i + quat_regularizer_1 +
+           controller_options_.quaternion_regularizer_fraction *
+               quat_regularizer_2 +
+           quat_regularizer_3);
+      discount_factor *= controller_options_.c3_options.gamma;
+    }
+  }
+}
+
 }  // namespace systems
 }  // namespace c3

systems/c3_controller.h

@@ -179,6 +179,15 @@ class C3Controller : public drake::systems::LeafSystem<double> {
   void OutputC3Intermediates(const drake::systems::Context<double>& context,
                              C3Output::C3Intermediates* c3_intermediates) const;
 
+  /**
+   * @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;
+
   // Input and output port indices.
   drake::systems::InputPortIndex lcs_desired_state_input_port_;
   drake::systems::InputPortIndex lcs_state_input_port_;
@@ -214,10 +223,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,12 @@ struct C3ControllerOptions {
   double solve_time_filter_alpha = 0.0;
   double publish_frequency = 100.0;  // Hz
 
+  std::vector<int>
+      quaternion_indices;          // Start indices for each quaternion block
+  double quaternion_weight = 0.0;  // Quaternion cost scaling term
+  double quaternion_regularizer_fraction =
+      0.0;  // Outer product regularization scaling term
+
   std::vector<C3StatePredictionJoint> state_prediction_joints;
 
   template <typename Archive>
@@ -70,6 +76,9 @@ 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_indices));
+    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");