From 44a0503f2ea2858bd65b3a0ccaed6c8e07c78778 Mon Sep 17 00:00:00 2001 From: cuie23 Date: Fri, 17 Oct 2025 15:20:35 -0400 Subject: [PATCH 1/8] Add quaternion cost terms --- systems/BUILD.bazel | 13 +++ systems/c3_controller.cc | 51 +++++++++ systems/c3_controller.h | 11 +- systems/c3_controller_options.h | 7 ++ systems/common/quaternion_error_hessian.cc | 126 +++++++++++++++++++++ systems/common/quaternion_error_hessian.h | 18 +++ systems/test/quaternion_cost_test.cc | 0 7 files changed, 222 insertions(+), 4 deletions(-) create mode 100644 systems/common/quaternion_error_hessian.cc create mode 100644 systems/common/quaternion_error_hessian.h create mode 100644 systems/test/quaternion_cost_test.cc diff --git a/systems/BUILD.bazel b/systems/BUILD.bazel index 29eb400..aa67ab0 100644 --- a/systems/BUILD.bazel +++ b/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", diff --git a/systems/c3_controller.cc b/systems/c3_controller.cc index 85d4315..80a0c4e 100644 --- a/systems/c3_controller.cc +++ b/systems/c3_controller.cc @@ -7,6 +7,7 @@ #include "core/c3_miqp.h" #include "core/c3_qp.h" #include "multibody/lcs_factory.h" +#include "common/quaternion_error_hessian.h" #include "drake/common/text_logging.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,50 @@ 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 diff --git a/systems/c3_controller.h b/systems/c3_controller.h index 8a8df72..70af004 100644 --- a/systems/c3_controller.h +++ b/systems/c3_controller.h @@ -179,6 +179,9 @@ class C3Controller : public drake::systems::LeafSystem { void OutputC3Intermediates(const drake::systems::Context& context, C3Output::C3Intermediates* c3_intermediates) const; + 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 +217,10 @@ class C3Controller : public drake::systems::LeafSystem { std::vector state_prediction_joints_; // Cost matrices for optimization. - std::vector Q_; ///< State cost matrices. - std::vector R_; ///< Input cost matrices. - std::vector G_; ///< State-input cross-term matrices. - std::vector U_; ///< Constraint matrices. + mutable std::vector Q_; ///< State cost matrices. + mutable std::vector R_; ///< Input cost matrices. + mutable std::vector G_; ///< State-input cross-term matrices. + mutable std::vector U_; ///< Constraint matrices. int N_; ///< Horizon length. }; diff --git a/systems/c3_controller_options.h b/systems/c3_controller_options.h index 52bdea3..6cdee27 100644 --- a/systems/c3_controller_options.h +++ b/systems/c3_controller_options.h @@ -60,6 +60,10 @@ struct C3ControllerOptions { double solve_time_filter_alpha = 0.0; double publish_frequency = 100.0; // Hz + std::vector 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 state_prediction_joints; template @@ -70,6 +74,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"); diff --git a/systems/common/quaternion_error_hessian.cc b/systems/common/quaternion_error_hessian.cc new file mode 100644 index 0000000..2b8c5d2 --- /dev/null +++ b/systems/common/quaternion_error_hessian.cc @@ -0,0 +1,126 @@ +#include "quaternion_error_hessian.h" +#include + + +using Eigen::VectorXd; +using Eigen::MatrixXd; + +namespace c3 { +namespace systems { +namespace common { + +Eigen::MatrixXd hessian_of_squared_quaternion_angle_difference( + const Eigen::VectorXd& quat, const Eigen::VectorXd& quat_desired) +{ + // Check the inputs are of expected shape. + DRAKE_DEMAND(quat.size() == 4); + DRAKE_DEMAND(quat_desired.size() == 4); + + // If difference is very small set to 0 matrix to avoid NaN's + if ((quat - quat_desired).norm() < 1e-12 || + std::abs(quat.dot(quat_desired) - 1.0) < 1e-12) { + return Eigen::MatrixXd::Zero(4, 4); + } + + // Extract the quaternion components. + double q_w = quat(0); + double q_x = quat(1); + double q_y = quat(2); + double q_z = quat(3); + + double r_w = quat_desired(0); + double r_x = quat_desired(1); + double r_y = quat_desired(2); + double r_z = quat_desired(3); + + // Define reusable expressions. + double exp_1 = std::atan2( + std::sqrt(std::pow(q_w*r_x - q_x*r_w + q_y*r_z - q_z*r_y, 2) + + std::pow(q_w*r_y - q_x*r_z - q_y*r_w + q_z*r_x, 2) + + std::pow(q_w*r_z + q_x*r_y - q_y*r_x - q_z*r_w, 2)), + q_w*r_w + q_x*r_x + q_y*r_y + q_z*r_z); + double exp_2 = std::pow(q_w, 2)*std::pow(r_x, 2) + + std::pow(q_w, 2)*std::pow(r_y, 2) + std::pow(q_w, 2)*std::pow(r_z, 2) - + 2*q_w*q_x*r_w*r_x - 2*q_w*q_y*r_w*r_y - 2*q_w*q_z*r_w*r_z + + std::pow(q_x, 2)*std::pow(r_w, 2) + std::pow(q_x, 2)*std::pow(r_y, 2) + + std::pow(q_x, 2)*std::pow(r_z, 2) - + 2*q_x*q_y*r_x*r_y - 2*q_x*q_z*r_x*r_z; + double exp_3 = std::pow(q_y, 2)*std::pow(r_w, 2) + + std::pow(q_y, 2)*std::pow(r_x, 2) + std::pow(q_y, 2)*std::pow(r_z, 2) - + 2*q_y*q_z*r_y*r_z + std::pow(q_z, 2)*std::pow(r_w, 2) + + std::pow(q_z, 2)*std::pow(r_x, 2) + std::pow(q_z, 2)*std::pow(r_y, 2); + double exp_4 = std::pow(q_w, 2) + std::pow(q_x, 2) + + std::pow(q_y, 2) + std::pow(q_z, 2); + double exp_5 = std::pow(exp_4, 2)*std::pow(exp_2 + exp_3, 5/2); + double exp_6 = std::pow(exp_2 + exp_3, 3/2); + double exp_7 = q_w*q_x*r_x + q_w*q_y*r_y + q_w*q_z*r_z - + std::pow(q_x, 2)*r_w - std::pow(q_y, 2)*r_w - std::pow(q_z, 2)*r_w; + double exp_8 = std::pow(q_w, 2)*r_y - q_w*q_y*r_w + std::pow(q_x, 2)*r_y - + q_x*q_y*r_x - q_y*q_z*r_z + std::pow(q_z, 2)*r_y; + double exp_9 = std::pow(q_w, 2)*r_x - q_w*q_x*r_w - q_x*q_y*r_y - + q_x*q_z*r_z + std::pow(q_y, 2)*r_x + std::pow(q_z, 2)*r_x; + double exp_10 = q_w*r_w*r_z + q_x*r_x*r_z + q_y*r_y*r_z - + q_z*std::pow(r_w, 2) - q_z*std::pow(r_x, 2) - q_z*std::pow(r_y, 2); + double exp_11 = std::pow(q_w, 2)*r_z - q_w*q_z*r_w + std::pow(q_x, 2)*r_z - + q_x*q_z*r_x + std::pow(q_y, 2)*r_z - q_y*q_z*r_y; + double exp_12 = q_w*r_w*r_y + q_x*r_x*r_y - q_y*std::pow(r_w, 2) - + q_y*std::pow(r_x, 2) - q_y*std::pow(r_z, 2) + q_z*r_y*r_z; + double exp_13 = q_w*r_w*r_x - q_x*std::pow(r_w, 2) - q_x*std::pow(r_y, 2) - + q_x*std::pow(r_z, 2) + q_y*r_x*r_y + q_z*r_x*r_z; + + // Define the Hessian elements. + double H_ww = 8*(-(2*q_w*(exp_7)*(exp_2 + exp_3) - + (q_x*r_x + q_y*r_y + q_z*r_z)*(exp_4)*(exp_2 + exp_3) + + (exp_4)*(q_w*std::pow(r_x, 2) + q_w*std::pow(r_y, 2) + + q_w*std::pow(r_z, 2) - q_x*r_w*r_x - q_y*r_w*r_y - q_z*r_w*r_z)* + (exp_7))*(exp_2 + exp_3)*exp_1 + std::pow(exp_7, 2)*(exp_6))/(exp_5); + double H_xx = 8*((2*q_x*(exp_9)*(exp_2 + exp_3) + + (q_w*r_w + q_y*r_y + q_z*r_z)*(exp_4)*(exp_2 + exp_3) - + (exp_4)*(exp_9)*(exp_13))*(exp_2 + exp_3)*exp_1 + + std::pow(exp_9, 2)*(exp_6))/(exp_5); + double H_yy = 8*((2*q_y*(exp_8)*(exp_2 + exp_3) + + (q_w*r_w + q_x*r_x + q_z*r_z)*(exp_4)*(exp_2 + exp_3) - + (exp_4)*(exp_8)*(exp_12))*(exp_2 + exp_3)*exp_1 + + std::pow(exp_8, 2)*(exp_6))/(exp_5); + double H_zz = 8*((2*q_z*(exp_11)*(exp_2 + exp_3) + + (q_w*r_w + q_x*r_x + q_y*r_y)*(exp_4)*(exp_2 + exp_3) - + (exp_4)*(exp_11)*((exp_10)))*(exp_2 + exp_3)*exp_1 + + std::pow(exp_11, 2)*(exp_6))/(exp_5); + double H_wx = 8*((-2*q_x*(exp_7)*(exp_2 + exp_3) + + (q_w*r_x - 2*q_x*r_w)*(exp_4)*(exp_2 + exp_3) + + (exp_4)*(exp_7)*(exp_13))*(exp_2 + exp_3)*exp_1 - + (exp_9)*(exp_7)*(exp_6))/(exp_5); + double H_wy = 8*((-2*q_y*(exp_7)*(exp_2 + exp_3) + + (q_w*r_y - 2*q_y*r_w)*(exp_4)*(exp_2 + exp_3) + + (exp_4)*(exp_7)*(exp_12))*(exp_2 + exp_3)*exp_1 - + (exp_8)*(exp_7)*(exp_6))/(exp_5); + double H_wz = 8*((-2*q_z*(exp_7)*(exp_2 + exp_3) + + (q_w*r_z - 2*q_z*r_w)*(exp_4)*(exp_2 + exp_3) + + (exp_4)*(exp_7)*((exp_10)))*(exp_2 + exp_3)*exp_1 - + (exp_11)*(exp_7)*(exp_6))/(exp_5); + double H_xy = 8*((2*q_y*(exp_9)*(exp_2 + exp_3) + + (q_x*r_y - 2*q_y*r_x)*(exp_4)*(exp_2 + exp_3) - + (exp_4)*(exp_9)*(exp_12))*(exp_2 + exp_3)*exp_1 + + (exp_9)*(exp_8)*(exp_6))/(exp_5); + double H_xz = 8*((2*q_z*(exp_9)*(exp_2 + exp_3) + + (q_x*r_z - 2*q_z*r_x)*(exp_4)*(exp_2 + exp_3) - + (exp_4)*(exp_9)*((exp_10)))*(exp_2 + exp_3)*exp_1 + + (exp_9)*(exp_11)*(exp_6))/(exp_5); + double H_yz = 8*((2*q_z*(exp_8)*(exp_2 + exp_3) + + (q_y*r_z - 2*q_z*r_y)*(exp_4)*(exp_2 + exp_3) - + (exp_4)*(exp_8)*((exp_10)))*(exp_2 + exp_3)*exp_1 + + (exp_8)*(exp_11)*(exp_6))/(exp_5); + + // Define the Hessian. + Eigen::MatrixXd hessian = Eigen::MatrixXd::Zero(4, 4); + hessian.col(0) << H_ww, H_wx, H_wy, H_wz; + hessian.col(1) << H_wx, H_xx, H_xy, H_xz; + hessian.col(2) << H_wy, H_xy, H_yy, H_yz; + hessian.col(3) << H_wz, H_xz, H_yz, H_zz; + + return hessian; +} + +} // namespace common +} // namespace systems +} // namespace c3 \ No newline at end of file diff --git a/systems/common/quaternion_error_hessian.h b/systems/common/quaternion_error_hessian.h new file mode 100644 index 0000000..bd87e46 --- /dev/null +++ b/systems/common/quaternion_error_hessian.h @@ -0,0 +1,18 @@ +#include +#include +#include "drake/common/drake_assert.h" + +using Eigen::VectorXd; +using Eigen::MatrixXd; + +namespace c3 { +namespace systems { +namespace common { + +Eigen::MatrixXd hessian_of_squared_quaternion_angle_difference( + const Eigen::VectorXd& quat, + const Eigen::VectorXd& quat_desired); + +} // namespace common +} // namespace systems +} // namespace c3 \ No newline at end of file diff --git a/systems/test/quaternion_cost_test.cc b/systems/test/quaternion_cost_test.cc new file mode 100644 index 0000000..e69de29 From 5088493ab51aa2aacac0e1f2e7f061c0a8d8d634 Mon Sep 17 00:00:00 2001 From: cuie23 Date: Fri, 17 Oct 2025 16:50:54 -0400 Subject: [PATCH 2/8] Added test cases for quaternion cost --- systems/BUILD.bazel | 14 +++ systems/test/quaternion_cost_test.cc | 142 +++++++++++++++++++++++++ systems/test/quaternion_cost_test.yaml | 71 +++++++++++++ 3 files changed, 227 insertions(+) create mode 100644 systems/test/quaternion_cost_test.yaml diff --git a/systems/BUILD.bazel b/systems/BUILD.bazel index aa67ab0..aeed271 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -144,6 +144,20 @@ cc_test( ], ) +cc_test( + name = "quaternion_cost_test", + srcs = [ + "test/quaternion_cost_test.cc" + ], + data = [ + "test/quaternion_cost_test.yaml" + ], + deps = [ + ":systems", + "@gtest//:main", + ], +) + filegroup( name = "headers", srcs = glob([ diff --git a/systems/test/quaternion_cost_test.cc b/systems/test/quaternion_cost_test.cc index e69de29..c6894d9 100644 --- a/systems/test/quaternion_cost_test.cc +++ b/systems/test/quaternion_cost_test.cc @@ -0,0 +1,142 @@ +#include +#include +#include + +#include +#include + +#include "systems/common/quaternion_error_hessian.h" +#include "systems/c3_controller.h" + +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::vector; +using drake::systems::DiagramBuilder; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::MultibodyPlant; + +namespace c3 { +namespace systems { +namespace test { + +class QuaternionCostTest : public ::testing::Test { + protected: + void SetUp() override {} + + // Copy function from c3 controller to test, slightly modified to return Q + std::vector UpdateQuaternionCosts( + const Eigen::VectorXd& x_curr, const Eigen::VectorXd& x_des, + C3ControllerOptions controller_options) const { + std::vector Q; + std::vector R; + std::vector G; + std::vector U; + + double discount_factor = 1; + for (int i = 0; i < controller_options.lcs_factory_options.N; i++) { + Q.push_back(discount_factor * controller_options.c3_options.Q); + discount_factor *= controller_options.c3_options.gamma; + if (i < controller_options.lcs_factory_options.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 < controller_options.lcs_factory_options.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; + } + } + return Q; + } +}; + +TEST_F(QuaternionCostTest, HessianAngleDifferenceTest) { + // Construct from initializer list. + VectorXd q1(4); + q1 << 0.5, 0.5, 0.5, 0.5 ; + + MatrixXd hessian = common::hessian_of_squared_quaternion_angle_difference(q1, q1); + + EXPECT_EQ(hessian.rows(), 4); + EXPECT_EQ(hessian.cols(), 4); + EXPECT_TRUE(hessian.isApprox(Eigen::MatrixXd::Zero(4, 4))); + + VectorXd q2(4); + q2 << 0.707, 0, 0, 0.707; + + hessian = common::hessian_of_squared_quaternion_angle_difference(q1, q2); + + EXPECT_EQ(hessian.rows(), 4); + EXPECT_EQ(hessian.cols(), 4); + + MatrixXd true_hessian(4, 4); + true_hessian << 6.4422, -2, -2, 2, + -2, 2, -2.4422, -2, + -2, -2.4422, 2, -2, + 2, -2, -2, 6.4422; + + + EXPECT_TRUE(hessian.isApprox(true_hessian, 1e-4)); + +} + + + +TEST_F(QuaternionCostTest, QuaternionCostMatrixTest) { + C3ControllerOptions controller_options = + drake::yaml::LoadYamlFile( + "systems/test/quaternion_cost_test.yaml"); + + VectorXd q1(4); + q1 << 0.5, 0.5, 0.5, 0.5; + VectorXd q2(4); + q2 << 0.707, 0, 0, 0.707; + + std::vector Q = UpdateQuaternionCosts(q1, q2, controller_options); + + for (int i = 0; i < Q.size(); i++) { + // Check each Q is PSD + double min_eigenval = Q[i].eigenvalues().real().minCoeff(); + EXPECT_TRUE(min_eigenval >= 0); + + // Check each expected block has been updated + std::vector start_indices = controller_options.quaternion_indices; + for (int idx : start_indices) { + MatrixXd Q_block = Q[i].block(idx, idx, 4, 4); + MatrixXd Q_diag = Q_block.diagonal().asDiagonal(); + EXPECT_TRUE(!(Q_block.isApprox(Q_diag))); + } + } + + + + + + +} + +} // namespace test +} // namespace systems +} // namespace c3 \ No newline at end of file diff --git a/systems/test/quaternion_cost_test.yaml b/systems/test/quaternion_cost_test.yaml new file mode 100644 index 0000000..36c686a --- /dev/null +++ b/systems/test/quaternion_cost_test.yaml @@ -0,0 +1,71 @@ +# Example yaml parameter file for testing + +projection_type: "MIQP" + +solve_time_filter_alpha: 0.0 +#set to 0 to publish as fast as possible +publish_frequency: 100 +state_prediction_joints: [] + +quaternion_indices: [5] +quaternion_weight: 1000 +quaternion_regularizer_fraction: 0.0 + +lcs_factory_options: + # options are 'stewart_and_trinkle' or 'anitescu' + contact_model: "anitescu" + num_friction_directions: 2 + num_contacts: 8 + spring_stiffness: 0 + mu: [0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4] + N: 10 + dt: 0.02 + +c3_options: + warm_start: false + end_on_qp_step: true + scale_lcs: false + + num_threads: 10 + delta_option: 1 + + M: 1000 + + admm_iter: 3 + + gamma: 1.0 + rho_scale: 1.1 #matrix scaling + w_Q: 5 + w_R: 1 #Penalty on all decision variables, assuming scalar + w_G: 0.2 #Penalty on all decision variables, assuming scalar + w_U: 1 #State Tracking Error, assuming diagonal + + + q_vector: [1, 1, 1, + 1, 1, + 1, 1, 1, 1, + 500, 500, 500, + 1, 1, 1, + 1, 1, + 0.1, 0.1, 0.1, + 1, 1, 1] + + r_vector: [0.01, 0.01, 0.01, 0.01, 0.01] + + #Penalty on matching projected variables + g_x: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] + g_gamma: [1, 1, 1, 1, 1, 1, 1, 1] + g_lambda_n: [1, 1, 1, 1, 1, 1, 1, 1] + g_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] + g_lambda: [2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2] + g_u: [0, 0, 0, 0, 0] + + + + #Penalty on matching the QP variables + u_x: [100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100] + u_gamma: [1, 1, 1, 1, 1, 1, 1, 1] + u_lambda_n: [1, 1, 1, 1, 1, 1, 1, 1] + u_lambda_t: [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] + u_lambda: [2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2] + u_u: [0, 0, 0, 0, 0] From 4e9b3d4091f4e9199895e54df54f0654e5dbcdc7 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Fri, 17 Oct 2025 17:56:26 -0400 Subject: [PATCH 3/8] Avoid bazel test size warnings; minor documentation improvements --- core/BUILD.bazel | 1 + multibody/BUILD.bazel | 1 + systems/BUILD.bazel | 2 ++ systems/common/quaternion_error_hessian.cc | 14 +++++--------- systems/common/quaternion_error_hessian.h | 10 +++++++++- systems/test/quaternion_cost_test.yaml | 2 +- 6 files changed, 19 insertions(+), 11 deletions(-) diff --git a/core/BUILD.bazel b/core/BUILD.bazel index 5b80c66..d9cd6f1 100644 --- a/core/BUILD.bazel +++ b/core/BUILD.bazel @@ -99,6 +99,7 @@ cc_test( "GUROBI_HOME", "GRB_LICENSE_FILE" ], + size = "small", ) filegroup( diff --git a/multibody/BUILD.bazel b/multibody/BUILD.bazel index 40203ee..f446985 100644 --- a/multibody/BUILD.bazel +++ b/multibody/BUILD.bazel @@ -56,6 +56,7 @@ cc_test( ":lcs_factory", "@gtest//:main", ], + size = "small", ) filegroup( diff --git a/systems/BUILD.bazel b/systems/BUILD.bazel index aeed271..3472ac5 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -142,6 +142,7 @@ cc_test( ":framework", "@gtest//:main", ], + size = "small", ) cc_test( @@ -156,6 +157,7 @@ cc_test( ":systems", "@gtest//:main", ], + size = "small", ) filegroup( diff --git a/systems/common/quaternion_error_hessian.cc b/systems/common/quaternion_error_hessian.cc index 2b8c5d2..e8a5226 100644 --- a/systems/common/quaternion_error_hessian.cc +++ b/systems/common/quaternion_error_hessian.cc @@ -1,16 +1,12 @@ #include "quaternion_error_hessian.h" -#include -using Eigen::VectorXd; -using Eigen::MatrixXd; - namespace c3 { namespace systems { namespace common { -Eigen::MatrixXd hessian_of_squared_quaternion_angle_difference( - const Eigen::VectorXd& quat, const Eigen::VectorXd& quat_desired) +MatrixXd hessian_of_squared_quaternion_angle_difference( + const VectorXd& quat, const VectorXd& quat_desired) { // Check the inputs are of expected shape. DRAKE_DEMAND(quat.size() == 4); @@ -19,7 +15,7 @@ Eigen::MatrixXd hessian_of_squared_quaternion_angle_difference( // If difference is very small set to 0 matrix to avoid NaN's if ((quat - quat_desired).norm() < 1e-12 || std::abs(quat.dot(quat_desired) - 1.0) < 1e-12) { - return Eigen::MatrixXd::Zero(4, 4); + return MatrixXd::Zero(4, 4); } // Extract the quaternion components. @@ -112,7 +108,7 @@ Eigen::MatrixXd hessian_of_squared_quaternion_angle_difference( (exp_8)*(exp_11)*(exp_6))/(exp_5); // Define the Hessian. - Eigen::MatrixXd hessian = Eigen::MatrixXd::Zero(4, 4); + MatrixXd hessian = Eigen::MatrixXd::Zero(4, 4); hessian.col(0) << H_ww, H_wx, H_wy, H_wz; hessian.col(1) << H_wx, H_xx, H_xy, H_xz; hessian.col(2) << H_wy, H_xy, H_yy, H_yz; @@ -123,4 +119,4 @@ Eigen::MatrixXd hessian_of_squared_quaternion_angle_difference( } // namespace common } // namespace systems -} // namespace c3 \ No newline at end of file +} // namespace c3 diff --git a/systems/common/quaternion_error_hessian.h b/systems/common/quaternion_error_hessian.h index bd87e46..e3f7ad7 100644 --- a/systems/common/quaternion_error_hessian.h +++ b/systems/common/quaternion_error_hessian.h @@ -9,10 +9,18 @@ namespace c3 { namespace systems { namespace common { +/*! + * Computes the Hessian of the squared angle difference between two quaternions, + * with respect to the elements of the first. + * @param quat The first quaternion (size 4 vector). The Hessian of the squared + * angle difference is with respect to this quaternion. + * @param quat_desired The second quaternion (size 4 vector). + * @return The 4x4 Hessian matrix of the squared angle difference. +*/ Eigen::MatrixXd hessian_of_squared_quaternion_angle_difference( const Eigen::VectorXd& quat, const Eigen::VectorXd& quat_desired); } // namespace common } // namespace systems -} // namespace c3 \ No newline at end of file +} // namespace c3 diff --git a/systems/test/quaternion_cost_test.yaml b/systems/test/quaternion_cost_test.yaml index 36c686a..cce4e0f 100644 --- a/systems/test/quaternion_cost_test.yaml +++ b/systems/test/quaternion_cost_test.yaml @@ -1,4 +1,4 @@ -# Example yaml parameter file for testing +# yaml parameter file for testing quaternion error Hessian computations projection_type: "MIQP" From 84b40162f624760b7eaf23ba379ddac919e2dc18 Mon Sep 17 00:00:00 2001 From: cuie23 Date: Mon, 20 Oct 2025 16:35:38 -0400 Subject: [PATCH 4/8] Added (GPT's) closed-form limit as fallback for equal quaternions --- systems/common/quaternion_error_hessian.cc | 38 +++++++++++++++++++--- systems/test/quaternion_cost_test.cc | 33 +++++++++---------- 2 files changed, 50 insertions(+), 21 deletions(-) diff --git a/systems/common/quaternion_error_hessian.cc b/systems/common/quaternion_error_hessian.cc index 2b8c5d2..b28d17d 100644 --- a/systems/common/quaternion_error_hessian.cc +++ b/systems/common/quaternion_error_hessian.cc @@ -9,6 +9,36 @@ namespace c3 { namespace systems { namespace common { +// GPT snippet +Eigen::Matrix4d small_angle_hessian_at(const Eigen::Vector4d& r_in) { + // Ensure r is unit (or normalize) + Eigen::Vector4d r = r_in.normalized(); + double r_w = r(0); + double r_x = r(1); + double r_y = r(2); + double r_z = r(3); + + // Build M_r (4x3) + Eigen::Matrix M; + M << -r_x, -r_y, -r_z, + r_w, -r_z, r_y, + r_z, r_w, -r_x, + -r_y, r_x, r_w; + M *= 0.5; + + // Left pseudoinverse for full-column-rank M_r: + Eigen::Matrix3d MtM = M.transpose() * M; // 3x3 + Eigen::Matrix3d MtM_inv = MtM.inverse(); // safe: MtM is positive definite for unit r + Eigen::Matrix Mplus = MtM_inv * M.transpose(); // 3x4 + + // Limit Hessian in quaternion coords + Eigen::Matrix4d H = 2.0 * (Mplus.transpose() * Mplus); // 4x4 + + // Force symmetry to avoid tiny numerical asymmetry + H = 0.5 * (H + H.transpose()); + return H; +} + Eigen::MatrixXd hessian_of_squared_quaternion_angle_difference( const Eigen::VectorXd& quat, const Eigen::VectorXd& quat_desired) { @@ -16,10 +46,10 @@ Eigen::MatrixXd hessian_of_squared_quaternion_angle_difference( DRAKE_DEMAND(quat.size() == 4); DRAKE_DEMAND(quat_desired.size() == 4); - // If difference is very small set to 0 matrix to avoid NaN's + // If difference is very small set to closed-form limit to avoid NaN's if ((quat - quat_desired).norm() < 1e-12 || std::abs(quat.dot(quat_desired) - 1.0) < 1e-12) { - return Eigen::MatrixXd::Zero(4, 4); + return small_angle_hessian_at(quat); } // Extract the quaternion components. @@ -51,8 +81,8 @@ Eigen::MatrixXd hessian_of_squared_quaternion_angle_difference( std::pow(q_z, 2)*std::pow(r_x, 2) + std::pow(q_z, 2)*std::pow(r_y, 2); double exp_4 = std::pow(q_w, 2) + std::pow(q_x, 2) + std::pow(q_y, 2) + std::pow(q_z, 2); - double exp_5 = std::pow(exp_4, 2)*std::pow(exp_2 + exp_3, 5/2); - double exp_6 = std::pow(exp_2 + exp_3, 3/2); + double exp_5 = std::pow(exp_4, 2)*std::pow(exp_2 + exp_3, 5.0/2.0); + double exp_6 = std::pow(exp_2 + exp_3, 3.0/2.0); double exp_7 = q_w*q_x*r_x + q_w*q_y*r_y + q_w*q_z*r_z - std::pow(q_x, 2)*r_w - std::pow(q_y, 2)*r_w - std::pow(q_z, 2)*r_w; double exp_8 = std::pow(q_w, 2)*r_y - q_w*q_y*r_w + std::pow(q_x, 2)*r_y - diff --git a/systems/test/quaternion_cost_test.cc b/systems/test/quaternion_cost_test.cc index c6894d9..75f12a2 100644 --- a/systems/test/quaternion_cost_test.cc +++ b/systems/test/quaternion_cost_test.cc @@ -75,31 +75,35 @@ class QuaternionCostTest : public ::testing::Test { TEST_F(QuaternionCostTest, HessianAngleDifferenceTest) { // Construct from initializer list. VectorXd q1(4); - q1 << 0.5, 0.5, 0.5, 0.5 ; + q1 << 0.5, 0.5, 0.5, 0.5; MatrixXd hessian = common::hessian_of_squared_quaternion_angle_difference(q1, q1); + std::cout << hessian << std::endl; + + MatrixXd true_hessian(4, 4); + true_hessian << 6, -2, -2, -2, + -2, 6, -2, -2, + -2, -2, 6, -2, + -2, -2, -2, 6; EXPECT_EQ(hessian.rows(), 4); EXPECT_EQ(hessian.cols(), 4); - EXPECT_TRUE(hessian.isApprox(Eigen::MatrixXd::Zero(4, 4))); + EXPECT_TRUE(hessian.isApprox(true_hessian, 1e-4)); VectorXd q2(4); q2 << 0.707, 0, 0, 0.707; hessian = common::hessian_of_squared_quaternion_angle_difference(q1, q2); + std::cout << hessian << std::endl; + + true_hessian << 8.28319, -2, -2, 2, + -2, 2, -4.28319, -2, + -2, -4.28319, 2, -2, + 2, -2, -2, 8.28319; EXPECT_EQ(hessian.rows(), 4); EXPECT_EQ(hessian.cols(), 4); - - MatrixXd true_hessian(4, 4); - true_hessian << 6.4422, -2, -2, 2, - -2, 2, -2.4422, -2, - -2, -2.4422, 2, -2, - 2, -2, -2, 6.4422; - - EXPECT_TRUE(hessian.isApprox(true_hessian, 1e-4)); - } @@ -115,7 +119,7 @@ TEST_F(QuaternionCostTest, QuaternionCostMatrixTest) { q2 << 0.707, 0, 0, 0.707; std::vector Q = UpdateQuaternionCosts(q1, q2, controller_options); - + for (int i = 0; i < Q.size(); i++) { // Check each Q is PSD double min_eigenval = Q[i].eigenvalues().real().minCoeff(); @@ -129,11 +133,6 @@ TEST_F(QuaternionCostTest, QuaternionCostMatrixTest) { EXPECT_TRUE(!(Q_block.isApprox(Q_diag))); } } - - - - - } From 60370588228c111eb25d410b9f77340fc67eff41 Mon Sep 17 00:00:00 2001 From: cuie23 Date: Mon, 20 Oct 2025 16:39:33 -0400 Subject: [PATCH 5/8] minor tweaks --- core/BUILD.bazel | 3 ++- multibody/BUILD.bazel | 1 + systems/BUILD.bazel | 2 ++ systems/common/quaternion_error_hessian.cc | 8 ++------ systems/test/quaternion_cost_test.yaml | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/core/BUILD.bazel b/core/BUILD.bazel index 5b80c66..1ca82c1 100644 --- a/core/BUILD.bazel +++ b/core/BUILD.bazel @@ -97,8 +97,9 @@ cc_test( ], env_inherit = [ "GUROBI_HOME", - "GRB_LICENSE_FILE" + "GRB_LICENSE0_FILE" ], + size = "small", ) filegroup( diff --git a/multibody/BUILD.bazel b/multibody/BUILD.bazel index 40203ee..f446985 100644 --- a/multibody/BUILD.bazel +++ b/multibody/BUILD.bazel @@ -56,6 +56,7 @@ cc_test( ":lcs_factory", "@gtest//:main", ], + size = "small", ) filegroup( diff --git a/systems/BUILD.bazel b/systems/BUILD.bazel index aeed271..7edd2c6 100644 --- a/systems/BUILD.bazel +++ b/systems/BUILD.bazel @@ -130,6 +130,7 @@ cc_test( "GUROBI_HOME", "GRB_LICENSE_FILE" ], + size = "small", ) cc_test( @@ -142,6 +143,7 @@ cc_test( ":framework", "@gtest//:main", ], + size = "small", ) cc_test( diff --git a/systems/common/quaternion_error_hessian.cc b/systems/common/quaternion_error_hessian.cc index b28d17d..d0e55c7 100644 --- a/systems/common/quaternion_error_hessian.cc +++ b/systems/common/quaternion_error_hessian.cc @@ -1,10 +1,6 @@ #include "quaternion_error_hessian.h" -#include -using Eigen::VectorXd; -using Eigen::MatrixXd; - namespace c3 { namespace systems { namespace common { @@ -39,8 +35,8 @@ Eigen::Matrix4d small_angle_hessian_at(const Eigen::Vector4d& r_in) { return H; } -Eigen::MatrixXd hessian_of_squared_quaternion_angle_difference( - const Eigen::VectorXd& quat, const Eigen::VectorXd& quat_desired) +MatrixXd hessian_of_squared_quaternion_angle_difference( + const VectorXd& quat, const VectorXd& quat_desired) { // Check the inputs are of expected shape. DRAKE_DEMAND(quat.size() == 4); diff --git a/systems/test/quaternion_cost_test.yaml b/systems/test/quaternion_cost_test.yaml index 36c686a..cce4e0f 100644 --- a/systems/test/quaternion_cost_test.yaml +++ b/systems/test/quaternion_cost_test.yaml @@ -1,4 +1,4 @@ -# Example yaml parameter file for testing +# yaml parameter file for testing quaternion error Hessian computations projection_type: "MIQP" From b11e3563a5001e07033a7dfb15f032a21cac7a4f Mon Sep 17 00:00:00 2001 From: cuie23 Date: Mon, 20 Oct 2025 16:45:36 -0400 Subject: [PATCH 6/8] typo --- core/BUILD.bazel | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/BUILD.bazel b/core/BUILD.bazel index 1ca82c1..d9cd6f1 100644 --- a/core/BUILD.bazel +++ b/core/BUILD.bazel @@ -97,7 +97,7 @@ cc_test( ], env_inherit = [ "GUROBI_HOME", - "GRB_LICENSE0_FILE" + "GRB_LICENSE_FILE" ], size = "small", ) From 2c48dd563c603840abab78d2cee4650f1ccbe736 Mon Sep 17 00:00:00 2001 From: cuie23 Date: Wed, 22 Oct 2025 09:18:18 -0400 Subject: [PATCH 7/8] Added small angle difference test --- systems/common/quaternion_error_hessian.cc | 5 +- systems/test/quaternion_cost_test.cc | 78 +++++++++++++++++++++- 2 files changed, 79 insertions(+), 4 deletions(-) diff --git a/systems/common/quaternion_error_hessian.cc b/systems/common/quaternion_error_hessian.cc index 081e5ba..a1619a8 100644 --- a/systems/common/quaternion_error_hessian.cc +++ b/systems/common/quaternion_error_hessian.cc @@ -1,6 +1,5 @@ #include "quaternion_error_hessian.h" - namespace c3 { namespace systems { namespace common { @@ -43,8 +42,8 @@ MatrixXd hessian_of_squared_quaternion_angle_difference( DRAKE_DEMAND(quat_desired.size() == 4); // If difference is very small set to closed-form limit to avoid NaN's - if ((quat - quat_desired).norm() < 1e-12 || - std::abs(quat.dot(quat_desired) - 1.0) < 1e-12) { + if ((quat - quat_desired).norm() < 1e-3 || + std::abs(quat.dot(quat_desired) - 1.0) < 1e-3) { return small_angle_hessian_at(quat); } diff --git a/systems/test/quaternion_cost_test.cc b/systems/test/quaternion_cost_test.cc index 75f12a2..db9e300 100644 --- a/systems/test/quaternion_cost_test.cc +++ b/systems/test/quaternion_cost_test.cc @@ -135,7 +135,83 @@ TEST_F(QuaternionCostTest, QuaternionCostMatrixTest) { } } - +TEST_F(QuaternionCostTest, HessianSmallAngleDifferenceTest) { + VectorXd epsilon1(4); + epsilon1 << 0.001, 0, 0, 0; + + VectorXd epsilon2(4); + epsilon2 << 0, 0.001, 0, 0 ; + + VectorXd epsilon3(4); + epsilon3 << 0, 0, 0.001, 0; + + VectorXd epsilon4(4); + epsilon4 << 0, 0, 0, 0.001; + + for (int i = 0; i < 10; i++) { + double roll = 0.6 * i; + double pitch = -0.13 * i; + double yaw = 0.44 * i; + + Eigen::Quaterniond q = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); + + Eigen::Vector4d q_vec(q.w(), q.x(), q.y(), q.z()); + + MatrixXd hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec); + + MatrixXd peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon1); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian + << "\n perturbed_hessian:\n" << peturbed_hessian + << "\n difference:\n" << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon2); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian + << "\n perturbed_hessian:\n" << peturbed_hessian + << "\n difference:\n" << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon3); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian + << "\n perturbed_hessian:\n" << peturbed_hessian + << "\n difference:\n" << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon4); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian + << "\n perturbed_hessian:\n" << peturbed_hessian + << "\n difference:\n" << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon1 + epsilon2); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian + << "\n perturbed_hessian:\n" << peturbed_hessian + << "\n difference:\n" << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon2 + epsilon3); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian + << "\n perturbed_hessian:\n" << peturbed_hessian + << "\n difference:\n" << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon3 + epsilon4); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian + << "\n perturbed_hessian:\n" << peturbed_hessian + << "\n difference:\n" << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon4 + epsilon1); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian + << "\n perturbed_hessian:\n" << peturbed_hessian + << "\n difference:\n" << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon3 + epsilon1); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian + << "\n perturbed_hessian:\n" << peturbed_hessian + << "\n difference:\n" << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon4 + epsilon2); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian + << "\n perturbed_hessian:\n" << peturbed_hessian + << "\n difference:\n" << (hessian - peturbed_hessian); + } +} } // namespace test } // namespace systems } // namespace c3 \ No newline at end of file From f67d1a050545544196db109b92ba6503fb78f835 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Thu, 23 Oct 2025 10:56:59 -0400 Subject: [PATCH 8/8] Fix broken other examples due to adding new C3 controller parameters; fix formatting --- bindings/pyc3/c3_systems_py.cc | 10 +- .../c3_controller_cartpole_options.yaml | 5 + .../c3_controller_pivoting_options.yaml | 5 + systems/c3_controller.cc | 88 ++--- systems/c3_controller.h | 13 +- systems/c3_controller_options.h | 12 +- systems/common/quaternion_error_hessian.cc | 303 ++++++++++-------- systems/common/quaternion_error_hessian.h | 16 +- systems/test/quaternion_cost_test.cc | 222 +++++++------ systems/test/quaternion_cost_test.yaml | 1 + 10 files changed, 390 insertions(+), 285 deletions(-) diff --git a/bindings/pyc3/c3_systems_py.cc b/bindings/pyc3/c3_systems_py.cc index cc12670..aa55915 100644 --- a/bindings/pyc3/c3_systems_py.cc +++ b/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 \ No newline at end of file +} // namespace c3 diff --git a/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml b/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml index 1f886fa..db3c15a 100644 --- a/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml +++ b/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' diff --git a/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml b/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml index 50b49bf..e88c9f5 100644 --- a/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml +++ b/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' diff --git a/systems/c3_controller.cc b/systems/c3_controller.cc index 80a0c4e..acaf3ae 100644 --- a/systems/c3_controller.cc +++ b/systems/c3_controller.cc @@ -4,10 +4,10 @@ #include +#include "common/quaternion_error_hessian.h" #include "core/c3_miqp.h" #include "core/c3_qp.h" #include "multibody/lcs_factory.h" -#include "common/quaternion_error_hessian.h" #include "drake/common/text_logging.h" @@ -163,7 +163,7 @@ drake::systems::EventStatus C3Controller::ComputePlan( 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); @@ -278,50 +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(); +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_; 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; - } + 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 diff --git a/systems/c3_controller.h b/systems/c3_controller.h index 70af004..d271c9b 100644 --- a/systems/c3_controller.h +++ b/systems/c3_controller.h @@ -179,8 +179,14 @@ class C3Controller : public drake::systems::LeafSystem { void OutputC3Intermediates(const drake::systems::Context& context, C3Output::C3Intermediates* c3_intermediates) const; - void UpdateQuaternionCosts( - const VectorXd& x_curr, const Eigen::VectorXd& x_des) 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_; @@ -219,7 +225,8 @@ class C3Controller : public drake::systems::LeafSystem { // Cost matrices for optimization. mutable std::vector Q_; ///< State cost matrices. mutable std::vector R_; ///< Input cost matrices. - mutable std::vector G_; ///< State-input cross-term matrices. + mutable std::vector + G_; ///< State-input cross-term matrices. mutable std::vector U_; ///< Constraint matrices. int N_; ///< Horizon length. diff --git a/systems/c3_controller_options.h b/systems/c3_controller_options.h index 6cdee27..9a761f4 100644 --- a/systems/c3_controller_options.h +++ b/systems/c3_controller_options.h @@ -60,9 +60,11 @@ struct C3ControllerOptions { double solve_time_filter_alpha = 0.0; double publish_frequency = 100.0; // Hz - std::vector 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 + 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 state_prediction_joints; @@ -74,8 +76,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_indices)); - a->Visit(DRAKE_NVP(quaternion_weight)); + a->Visit(DRAKE_NVP(quaternion_indices)); + a->Visit(DRAKE_NVP(quaternion_weight)); a->Visit(DRAKE_NVP(quaternion_regularizer_fraction)); if (projection_type == "QP") { diff --git a/systems/common/quaternion_error_hessian.cc b/systems/common/quaternion_error_hessian.cc index a1619a8..9f8d03d 100644 --- a/systems/common/quaternion_error_hessian.cc +++ b/systems/common/quaternion_error_hessian.cc @@ -6,146 +6,191 @@ namespace common { // GPT snippet Eigen::Matrix4d small_angle_hessian_at(const Eigen::Vector4d& r_in) { - // Ensure r is unit (or normalize) - Eigen::Vector4d r = r_in.normalized(); - double r_w = r(0); - double r_x = r(1); - double r_y = r(2); - double r_z = r(3); + // Ensure r is unit (or normalize) + Eigen::Vector4d r = r_in.normalized(); + double r_w = r(0); + double r_x = r(1); + double r_y = r(2); + double r_z = r(3); - // Build M_r (4x3) - Eigen::Matrix M; - M << -r_x, -r_y, -r_z, - r_w, -r_z, r_y, - r_z, r_w, -r_x, - -r_y, r_x, r_w; - M *= 0.5; + // Build M_r (4x3) + Eigen::Matrix M; + M << -r_x, -r_y, -r_z, r_w, -r_z, r_y, r_z, r_w, -r_x, -r_y, r_x, r_w; + M *= 0.5; - // Left pseudoinverse for full-column-rank M_r: - Eigen::Matrix3d MtM = M.transpose() * M; // 3x3 - Eigen::Matrix3d MtM_inv = MtM.inverse(); // safe: MtM is PD for unit r - Eigen::Matrix Mplus = MtM_inv * M.transpose(); // 3x4 + // Left pseudoinverse for full-column-rank M_r: + Eigen::Matrix3d MtM = M.transpose() * M; // 3x3 + Eigen::Matrix3d MtM_inv = MtM.inverse(); // safe: MtM is PD for unit r + Eigen::Matrix Mplus = MtM_inv * M.transpose(); // 3x4 - // Limit Hessian in quaternion coords - Eigen::Matrix4d H = 2.0 * (Mplus.transpose() * Mplus); // 4x4 + // Limit Hessian in quaternion coords + Eigen::Matrix4d H = 2.0 * (Mplus.transpose() * Mplus); // 4x4 - // Force symmetry to avoid tiny numerical asymmetry - H = 0.5 * (H + H.transpose()); - return H; + // Force symmetry to avoid tiny numerical asymmetry + H = 0.5 * (H + H.transpose()); + return H; } - + MatrixXd hessian_of_squared_quaternion_angle_difference( - const VectorXd& quat, const VectorXd& quat_desired) -{ - // Check the inputs are of expected shape. - DRAKE_DEMAND(quat.size() == 4); - DRAKE_DEMAND(quat_desired.size() == 4); + const VectorXd& quat, const VectorXd& quat_desired) { + // Check the inputs are of expected shape. + DRAKE_DEMAND(quat.size() == 4); + DRAKE_DEMAND(quat_desired.size() == 4); + + // If difference is very small set to closed-form limit to avoid NaN's + if ((quat - quat_desired).norm() < 1e-3 || + std::abs(quat.dot(quat_desired) - 1.0) < 1e-3) { + return small_angle_hessian_at(quat); + } - // If difference is very small set to closed-form limit to avoid NaN's - if ((quat - quat_desired).norm() < 1e-3 || - std::abs(quat.dot(quat_desired) - 1.0) < 1e-3) { - return small_angle_hessian_at(quat); - } + // Extract the quaternion components. + double q_w = quat(0); + double q_x = quat(1); + double q_y = quat(2); + double q_z = quat(3); - // Extract the quaternion components. - double q_w = quat(0); - double q_x = quat(1); - double q_y = quat(2); - double q_z = quat(3); + double r_w = quat_desired(0); + double r_x = quat_desired(1); + double r_y = quat_desired(2); + double r_z = quat_desired(3); - double r_w = quat_desired(0); - double r_x = quat_desired(1); - double r_y = quat_desired(2); - double r_z = quat_desired(3); - - // Define reusable expressions. - double exp_1 = std::atan2( - std::sqrt(std::pow(q_w*r_x - q_x*r_w + q_y*r_z - q_z*r_y, 2) + - std::pow(q_w*r_y - q_x*r_z - q_y*r_w + q_z*r_x, 2) + - std::pow(q_w*r_z + q_x*r_y - q_y*r_x - q_z*r_w, 2)), - q_w*r_w + q_x*r_x + q_y*r_y + q_z*r_z); - double exp_2 = std::pow(q_w, 2)*std::pow(r_x, 2) + - std::pow(q_w, 2)*std::pow(r_y, 2) + std::pow(q_w, 2)*std::pow(r_z, 2) - - 2*q_w*q_x*r_w*r_x - 2*q_w*q_y*r_w*r_y - 2*q_w*q_z*r_w*r_z + - std::pow(q_x, 2)*std::pow(r_w, 2) + std::pow(q_x, 2)*std::pow(r_y, 2) + - std::pow(q_x, 2)*std::pow(r_z, 2) - - 2*q_x*q_y*r_x*r_y - 2*q_x*q_z*r_x*r_z; - double exp_3 = std::pow(q_y, 2)*std::pow(r_w, 2) + - std::pow(q_y, 2)*std::pow(r_x, 2) + std::pow(q_y, 2)*std::pow(r_z, 2) - - 2*q_y*q_z*r_y*r_z + std::pow(q_z, 2)*std::pow(r_w, 2) + - std::pow(q_z, 2)*std::pow(r_x, 2) + std::pow(q_z, 2)*std::pow(r_y, 2); - double exp_4 = std::pow(q_w, 2) + std::pow(q_x, 2) + - std::pow(q_y, 2) + std::pow(q_z, 2); - double exp_5 = std::pow(exp_4, 2)*std::pow(exp_2 + exp_3, 5.0/2.0); - double exp_6 = std::pow(exp_2 + exp_3, 3.0/2.0); - double exp_7 = q_w*q_x*r_x + q_w*q_y*r_y + q_w*q_z*r_z - - std::pow(q_x, 2)*r_w - std::pow(q_y, 2)*r_w - std::pow(q_z, 2)*r_w; - double exp_8 = std::pow(q_w, 2)*r_y - q_w*q_y*r_w + std::pow(q_x, 2)*r_y - - q_x*q_y*r_x - q_y*q_z*r_z + std::pow(q_z, 2)*r_y; - double exp_9 = std::pow(q_w, 2)*r_x - q_w*q_x*r_w - q_x*q_y*r_y - - q_x*q_z*r_z + std::pow(q_y, 2)*r_x + std::pow(q_z, 2)*r_x; - double exp_10 = q_w*r_w*r_z + q_x*r_x*r_z + q_y*r_y*r_z - - q_z*std::pow(r_w, 2) - q_z*std::pow(r_x, 2) - q_z*std::pow(r_y, 2); - double exp_11 = std::pow(q_w, 2)*r_z - q_w*q_z*r_w + std::pow(q_x, 2)*r_z - - q_x*q_z*r_x + std::pow(q_y, 2)*r_z - q_y*q_z*r_y; - double exp_12 = q_w*r_w*r_y + q_x*r_x*r_y - q_y*std::pow(r_w, 2) - - q_y*std::pow(r_x, 2) - q_y*std::pow(r_z, 2) + q_z*r_y*r_z; - double exp_13 = q_w*r_w*r_x - q_x*std::pow(r_w, 2) - q_x*std::pow(r_y, 2) - - q_x*std::pow(r_z, 2) + q_y*r_x*r_y + q_z*r_x*r_z; + // Define reusable expressions. + double exp_1 = std::atan2( + std::sqrt(std::pow(q_w * r_x - q_x * r_w + q_y * r_z - q_z * r_y, 2) + + std::pow(q_w * r_y - q_x * r_z - q_y * r_w + q_z * r_x, 2) + + std::pow(q_w * r_z + q_x * r_y - q_y * r_x - q_z * r_w, 2)), + q_w * r_w + q_x * r_x + q_y * r_y + q_z * r_z); + double exp_2 = std::pow(q_w, 2) * std::pow(r_x, 2) + + std::pow(q_w, 2) * std::pow(r_y, 2) + + std::pow(q_w, 2) * std::pow(r_z, 2) - + 2 * q_w * q_x * r_w * r_x - 2 * q_w * q_y * r_w * r_y - + 2 * q_w * q_z * r_w * r_z + + std::pow(q_x, 2) * std::pow(r_w, 2) + + std::pow(q_x, 2) * std::pow(r_y, 2) + + std::pow(q_x, 2) * std::pow(r_z, 2) - + 2 * q_x * q_y * r_x * r_y - 2 * q_x * q_z * r_x * r_z; + double exp_3 = + std::pow(q_y, 2) * std::pow(r_w, 2) + + std::pow(q_y, 2) * std::pow(r_x, 2) + + std::pow(q_y, 2) * std::pow(r_z, 2) - 2 * q_y * q_z * r_y * r_z + + std::pow(q_z, 2) * std::pow(r_w, 2) + + std::pow(q_z, 2) * std::pow(r_x, 2) + std::pow(q_z, 2) * std::pow(r_y, 2); + double exp_4 = + std::pow(q_w, 2) + std::pow(q_x, 2) + std::pow(q_y, 2) + std::pow(q_z, 2); + double exp_5 = std::pow(exp_4, 2) * std::pow(exp_2 + exp_3, 5.0 / 2.0); + double exp_6 = std::pow(exp_2 + exp_3, 3.0 / 2.0); + double exp_7 = q_w * q_x * r_x + q_w * q_y * r_y + q_w * q_z * r_z - + std::pow(q_x, 2) * r_w - std::pow(q_y, 2) * r_w - + std::pow(q_z, 2) * r_w; + double exp_8 = std::pow(q_w, 2) * r_y - q_w * q_y * r_w + + std::pow(q_x, 2) * r_y - q_x * q_y * r_x - q_y * q_z * r_z + + std::pow(q_z, 2) * r_y; + double exp_9 = std::pow(q_w, 2) * r_x - q_w * q_x * r_w - q_x * q_y * r_y - + q_x * q_z * r_z + std::pow(q_y, 2) * r_x + + std::pow(q_z, 2) * r_x; + double exp_10 = q_w * r_w * r_z + q_x * r_x * r_z + q_y * r_y * r_z - + q_z * std::pow(r_w, 2) - q_z * std::pow(r_x, 2) - + q_z * std::pow(r_y, 2); + double exp_11 = std::pow(q_w, 2) * r_z - q_w * q_z * r_w + + std::pow(q_x, 2) * r_z - q_x * q_z * r_x + + std::pow(q_y, 2) * r_z - q_y * q_z * r_y; + double exp_12 = q_w * r_w * r_y + q_x * r_x * r_y - q_y * std::pow(r_w, 2) - + q_y * std::pow(r_x, 2) - q_y * std::pow(r_z, 2) + + q_z * r_y * r_z; + double exp_13 = q_w * r_w * r_x - q_x * std::pow(r_w, 2) - + q_x * std::pow(r_y, 2) - q_x * std::pow(r_z, 2) + + q_y * r_x * r_y + q_z * r_x * r_z; - // Define the Hessian elements. - double H_ww = 8*(-(2*q_w*(exp_7)*(exp_2 + exp_3) - - (q_x*r_x + q_y*r_y + q_z*r_z)*(exp_4)*(exp_2 + exp_3) + - (exp_4)*(q_w*std::pow(r_x, 2) + q_w*std::pow(r_y, 2) + - q_w*std::pow(r_z, 2) - q_x*r_w*r_x - q_y*r_w*r_y - q_z*r_w*r_z)* - (exp_7))*(exp_2 + exp_3)*exp_1 + std::pow(exp_7, 2)*(exp_6))/(exp_5); - double H_xx = 8*((2*q_x*(exp_9)*(exp_2 + exp_3) + - (q_w*r_w + q_y*r_y + q_z*r_z)*(exp_4)*(exp_2 + exp_3) - - (exp_4)*(exp_9)*(exp_13))*(exp_2 + exp_3)*exp_1 + - std::pow(exp_9, 2)*(exp_6))/(exp_5); - double H_yy = 8*((2*q_y*(exp_8)*(exp_2 + exp_3) + - (q_w*r_w + q_x*r_x + q_z*r_z)*(exp_4)*(exp_2 + exp_3) - - (exp_4)*(exp_8)*(exp_12))*(exp_2 + exp_3)*exp_1 + - std::pow(exp_8, 2)*(exp_6))/(exp_5); - double H_zz = 8*((2*q_z*(exp_11)*(exp_2 + exp_3) + - (q_w*r_w + q_x*r_x + q_y*r_y)*(exp_4)*(exp_2 + exp_3) - - (exp_4)*(exp_11)*((exp_10)))*(exp_2 + exp_3)*exp_1 + - std::pow(exp_11, 2)*(exp_6))/(exp_5); - double H_wx = 8*((-2*q_x*(exp_7)*(exp_2 + exp_3) + - (q_w*r_x - 2*q_x*r_w)*(exp_4)*(exp_2 + exp_3) + - (exp_4)*(exp_7)*(exp_13))*(exp_2 + exp_3)*exp_1 - - (exp_9)*(exp_7)*(exp_6))/(exp_5); - double H_wy = 8*((-2*q_y*(exp_7)*(exp_2 + exp_3) + - (q_w*r_y - 2*q_y*r_w)*(exp_4)*(exp_2 + exp_3) + - (exp_4)*(exp_7)*(exp_12))*(exp_2 + exp_3)*exp_1 - - (exp_8)*(exp_7)*(exp_6))/(exp_5); - double H_wz = 8*((-2*q_z*(exp_7)*(exp_2 + exp_3) + - (q_w*r_z - 2*q_z*r_w)*(exp_4)*(exp_2 + exp_3) + - (exp_4)*(exp_7)*((exp_10)))*(exp_2 + exp_3)*exp_1 - - (exp_11)*(exp_7)*(exp_6))/(exp_5); - double H_xy = 8*((2*q_y*(exp_9)*(exp_2 + exp_3) + - (q_x*r_y - 2*q_y*r_x)*(exp_4)*(exp_2 + exp_3) - - (exp_4)*(exp_9)*(exp_12))*(exp_2 + exp_3)*exp_1 + - (exp_9)*(exp_8)*(exp_6))/(exp_5); - double H_xz = 8*((2*q_z*(exp_9)*(exp_2 + exp_3) + - (q_x*r_z - 2*q_z*r_x)*(exp_4)*(exp_2 + exp_3) - - (exp_4)*(exp_9)*((exp_10)))*(exp_2 + exp_3)*exp_1 + - (exp_9)*(exp_11)*(exp_6))/(exp_5); - double H_yz = 8*((2*q_z*(exp_8)*(exp_2 + exp_3) + - (q_y*r_z - 2*q_z*r_y)*(exp_4)*(exp_2 + exp_3) - - (exp_4)*(exp_8)*((exp_10)))*(exp_2 + exp_3)*exp_1 + - (exp_8)*(exp_11)*(exp_6))/(exp_5); + // Define the Hessian elements. + double H_ww = + 8 * + (-(2 * q_w * (exp_7) * (exp_2 + exp_3) - + (q_x * r_x + q_y * r_y + q_z * r_z) * (exp_4) * (exp_2 + exp_3) + + (exp_4) * + (q_w * std::pow(r_x, 2) + q_w * std::pow(r_y, 2) + + q_w * std::pow(r_z, 2) - q_x * r_w * r_x - q_y * r_w * r_y - + q_z * r_w * r_z) * + (exp_7)) * + (exp_2 + exp_3) * exp_1 + + std::pow(exp_7, 2) * (exp_6)) / + (exp_5); + double H_xx = + 8 * + ((2 * q_x * (exp_9) * (exp_2 + exp_3) + + (q_w * r_w + q_y * r_y + q_z * r_z) * (exp_4) * (exp_2 + exp_3) - + (exp_4) * (exp_9) * (exp_13)) * + (exp_2 + exp_3) * exp_1 + + std::pow(exp_9, 2) * (exp_6)) / + (exp_5); + double H_yy = + 8 * + ((2 * q_y * (exp_8) * (exp_2 + exp_3) + + (q_w * r_w + q_x * r_x + q_z * r_z) * (exp_4) * (exp_2 + exp_3) - + (exp_4) * (exp_8) * (exp_12)) * + (exp_2 + exp_3) * exp_1 + + std::pow(exp_8, 2) * (exp_6)) / + (exp_5); + double H_zz = + 8 * + ((2 * q_z * (exp_11) * (exp_2 + exp_3) + + (q_w * r_w + q_x * r_x + q_y * r_y) * (exp_4) * (exp_2 + exp_3) - + (exp_4) * (exp_11) * ((exp_10))) * + (exp_2 + exp_3) * exp_1 + + std::pow(exp_11, 2) * (exp_6)) / + (exp_5); + double H_wx = 8 * + ((-2 * q_x * (exp_7) * (exp_2 + exp_3) + + (q_w * r_x - 2 * q_x * r_w) * (exp_4) * (exp_2 + exp_3) + + (exp_4) * (exp_7) * (exp_13)) * + (exp_2 + exp_3) * exp_1 - + (exp_9) * (exp_7) * (exp_6)) / + (exp_5); + double H_wy = 8 * + ((-2 * q_y * (exp_7) * (exp_2 + exp_3) + + (q_w * r_y - 2 * q_y * r_w) * (exp_4) * (exp_2 + exp_3) + + (exp_4) * (exp_7) * (exp_12)) * + (exp_2 + exp_3) * exp_1 - + (exp_8) * (exp_7) * (exp_6)) / + (exp_5); + double H_wz = 8 * + ((-2 * q_z * (exp_7) * (exp_2 + exp_3) + + (q_w * r_z - 2 * q_z * r_w) * (exp_4) * (exp_2 + exp_3) + + (exp_4) * (exp_7) * ((exp_10))) * + (exp_2 + exp_3) * exp_1 - + (exp_11) * (exp_7) * (exp_6)) / + (exp_5); + double H_xy = 8 * + ((2 * q_y * (exp_9) * (exp_2 + exp_3) + + (q_x * r_y - 2 * q_y * r_x) * (exp_4) * (exp_2 + exp_3) - + (exp_4) * (exp_9) * (exp_12)) * + (exp_2 + exp_3) * exp_1 + + (exp_9) * (exp_8) * (exp_6)) / + (exp_5); + double H_xz = 8 * + ((2 * q_z * (exp_9) * (exp_2 + exp_3) + + (q_x * r_z - 2 * q_z * r_x) * (exp_4) * (exp_2 + exp_3) - + (exp_4) * (exp_9) * ((exp_10))) * + (exp_2 + exp_3) * exp_1 + + (exp_9) * (exp_11) * (exp_6)) / + (exp_5); + double H_yz = 8 * + ((2 * q_z * (exp_8) * (exp_2 + exp_3) + + (q_y * r_z - 2 * q_z * r_y) * (exp_4) * (exp_2 + exp_3) - + (exp_4) * (exp_8) * ((exp_10))) * + (exp_2 + exp_3) * exp_1 + + (exp_8) * (exp_11) * (exp_6)) / + (exp_5); - // Define the Hessian. - MatrixXd hessian = Eigen::MatrixXd::Zero(4, 4); - hessian.col(0) << H_ww, H_wx, H_wy, H_wz; - hessian.col(1) << H_wx, H_xx, H_xy, H_xz; - hessian.col(2) << H_wy, H_xy, H_yy, H_yz; - hessian.col(3) << H_wz, H_xz, H_yz, H_zz; + // Define the Hessian. + MatrixXd hessian = Eigen::MatrixXd::Zero(4, 4); + hessian.col(0) << H_ww, H_wx, H_wy, H_wz; + hessian.col(1) << H_wx, H_xx, H_xy, H_xz; + hessian.col(2) << H_wy, H_xy, H_yy, H_yz; + hessian.col(3) << H_wz, H_xz, H_yz, H_zz; - return hessian; + return hessian; } -} // namespace common -} // namespace systems -} // namespace c3 +} // namespace common +} // namespace systems +} // namespace c3 diff --git a/systems/common/quaternion_error_hessian.h b/systems/common/quaternion_error_hessian.h index e3f7ad7..5f99399 100644 --- a/systems/common/quaternion_error_hessian.h +++ b/systems/common/quaternion_error_hessian.h @@ -1,9 +1,10 @@ #include #include + #include "drake/common/drake_assert.h" -using Eigen::VectorXd; using Eigen::MatrixXd; +using Eigen::VectorXd; namespace c3 { namespace systems { @@ -16,11 +17,10 @@ namespace common { * angle difference is with respect to this quaternion. * @param quat_desired The second quaternion (size 4 vector). * @return The 4x4 Hessian matrix of the squared angle difference. -*/ -Eigen::MatrixXd hessian_of_squared_quaternion_angle_difference( - const Eigen::VectorXd& quat, - const Eigen::VectorXd& quat_desired); + */ +MatrixXd hessian_of_squared_quaternion_angle_difference( + const VectorXd& quat, const VectorXd& quat_desired); -} // namespace common -} // namespace systems -} // namespace c3 +} // namespace common +} // namespace systems +} // namespace c3 diff --git a/systems/test/quaternion_cost_test.cc b/systems/test/quaternion_cost_test.cc index db9e300..0c1d2d3 100644 --- a/systems/test/quaternion_cost_test.cc +++ b/systems/test/quaternion_cost_test.cc @@ -2,18 +2,18 @@ #include #include -#include #include +#include -#include "systems/common/quaternion_error_hessian.h" #include "systems/c3_controller.h" +#include "systems/common/quaternion_error_hessian.h" +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::MultibodyPlant; +using drake::systems::DiagramBuilder; using Eigen::MatrixXd; using Eigen::VectorXd; using std::vector; -using drake::systems::DiagramBuilder; -using drake::multibody::AddMultibodyPlantSceneGraph; -using drake::multibody::MultibodyPlant; namespace c3 { namespace systems { @@ -25,7 +25,7 @@ class QuaternionCostTest : public ::testing::Test { // Copy function from c3 controller to test, slightly modified to return Q std::vector UpdateQuaternionCosts( - const Eigen::VectorXd& x_curr, const Eigen::VectorXd& x_des, + const Eigen::VectorXd& x_curr, const Eigen::VectorXd& x_des, C3ControllerOptions controller_options) const { std::vector Q; std::vector R; @@ -34,37 +34,42 @@ class QuaternionCostTest : public ::testing::Test { double discount_factor = 1; for (int i = 0; i < controller_options.lcs_factory_options.N; i++) { - Q.push_back(discount_factor * controller_options.c3_options.Q); - discount_factor *= controller_options.c3_options.gamma; + Q.push_back(discount_factor * controller_options.c3_options.Q); + discount_factor *= controller_options.c3_options.gamma; if (i < controller_options.lcs_factory_options.N) { - R.push_back(discount_factor * controller_options.c3_options.R); + 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); + } + 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); + 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_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); + Eigen::MatrixXd quat_regularizer_3 = + 1e-8 * Eigen::MatrixXd::Identity(4, 4); double discount_factor = 1; for (int i = 0; i < controller_options.lcs_factory_options.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); + 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; } } @@ -77,49 +82,43 @@ TEST_F(QuaternionCostTest, HessianAngleDifferenceTest) { VectorXd q1(4); q1 << 0.5, 0.5, 0.5, 0.5; - MatrixXd hessian = common::hessian_of_squared_quaternion_angle_difference(q1, q1); + MatrixXd hessian = + common::hessian_of_squared_quaternion_angle_difference(q1, q1); std::cout << hessian << std::endl; MatrixXd true_hessian(4, 4); - true_hessian << 6, -2, -2, -2, - -2, 6, -2, -2, - -2, -2, 6, -2, - -2, -2, -2, 6; + true_hessian << 6, -2, -2, -2, -2, 6, -2, -2, -2, -2, 6, -2, -2, -2, -2, 6; EXPECT_EQ(hessian.rows(), 4); EXPECT_EQ(hessian.cols(), 4); EXPECT_TRUE(hessian.isApprox(true_hessian, 1e-4)); VectorXd q2(4); - q2 << 0.707, 0, 0, 0.707; + q2 << 0.707, 0, 0, 0.707; hessian = common::hessian_of_squared_quaternion_angle_difference(q1, q2); std::cout << hessian << std::endl; - true_hessian << 8.28319, -2, -2, 2, - -2, 2, -4.28319, -2, - -2, -4.28319, 2, -2, - 2, -2, -2, 8.28319; + true_hessian << 8.28319, -2, -2, 2, -2, 2, -4.28319, -2, -2, -4.28319, 2, -2, + 2, -2, -2, 8.28319; EXPECT_EQ(hessian.rows(), 4); EXPECT_EQ(hessian.cols(), 4); EXPECT_TRUE(hessian.isApprox(true_hessian, 1e-4)); } - - TEST_F(QuaternionCostTest, QuaternionCostMatrixTest) { C3ControllerOptions controller_options = - drake::yaml::LoadYamlFile( - "systems/test/quaternion_cost_test.yaml"); + drake::yaml::LoadYamlFile( + "systems/test/quaternion_cost_test.yaml"); VectorXd q1(4); q1 << 0.5, 0.5, 0.5, 0.5; VectorXd q2(4); - q2 << 0.707, 0, 0, 0.707; + q2 << 0.707, 0, 0, 0.707; std::vector Q = UpdateQuaternionCosts(q1, q2, controller_options); - + for (int i = 0; i < Q.size(); i++) { // Check each Q is PSD double min_eigenval = Q[i].eigenvalues().real().minCoeff(); @@ -129,89 +128,120 @@ TEST_F(QuaternionCostTest, QuaternionCostMatrixTest) { std::vector start_indices = controller_options.quaternion_indices; for (int idx : start_indices) { MatrixXd Q_block = Q[i].block(idx, idx, 4, 4); - MatrixXd Q_diag = Q_block.diagonal().asDiagonal(); + MatrixXd Q_diag = Q_block.diagonal().asDiagonal(); EXPECT_TRUE(!(Q_block.isApprox(Q_diag))); } - } - + } } TEST_F(QuaternionCostTest, HessianSmallAngleDifferenceTest) { VectorXd epsilon1(4); epsilon1 << 0.001, 0, 0, 0; VectorXd epsilon2(4); - epsilon2 << 0, 0.001, 0, 0 ; + epsilon2 << 0, 0.001, 0, 0; VectorXd epsilon3(4); epsilon3 << 0, 0, 0.001, 0; VectorXd epsilon4(4); epsilon4 << 0, 0, 0, 0.001; - + for (int i = 0; i < 10; i++) { double roll = 0.6 * i; double pitch = -0.13 * i; double yaw = 0.44 * i; - Eigen::Quaterniond q = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * + Eigen::Quaterniond q = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); + Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); Eigen::Vector4d q_vec(q.w(), q.x(), q.y(), q.z()); - MatrixXd hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec); - - MatrixXd peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon1); - EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian - << "\n perturbed_hessian:\n" << peturbed_hessian - << "\n difference:\n" << (hessian - peturbed_hessian); - - peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon2); - EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian - << "\n perturbed_hessian:\n" << peturbed_hessian - << "\n difference:\n" << (hessian - peturbed_hessian); - - peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon3); - EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian - << "\n perturbed_hessian:\n" << peturbed_hessian - << "\n difference:\n" << (hessian - peturbed_hessian); - - peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon4); - EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian - << "\n perturbed_hessian:\n" << peturbed_hessian - << "\n difference:\n" << (hessian - peturbed_hessian); - - peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon1 + epsilon2); - EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian - << "\n perturbed_hessian:\n" << peturbed_hessian - << "\n difference:\n" << (hessian - peturbed_hessian); - - peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon2 + epsilon3); - EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian - << "\n perturbed_hessian:\n" << peturbed_hessian - << "\n difference:\n" << (hessian - peturbed_hessian); - - peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon3 + epsilon4); - EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian - << "\n perturbed_hessian:\n" << peturbed_hessian - << "\n difference:\n" << (hessian - peturbed_hessian); - - peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon4 + epsilon1); - EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian - << "\n perturbed_hessian:\n" << peturbed_hessian - << "\n difference:\n" << (hessian - peturbed_hessian); - - peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon3 + epsilon1); - EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian - << "\n perturbed_hessian:\n" << peturbed_hessian - << "\n difference:\n" << (hessian - peturbed_hessian); - - peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec + epsilon4 + epsilon2); - EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2))<< "\n hessian:\n" << hessian - << "\n perturbed_hessian:\n" << peturbed_hessian - << "\n difference:\n" << (hessian - peturbed_hessian); + MatrixXd hessian = + common::hessian_of_squared_quaternion_angle_difference(q_vec, q_vec); + + MatrixXd peturbed_hessian = + common::hessian_of_squared_quaternion_angle_difference( + q_vec, q_vec + epsilon1); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2)) + << "\n hessian:\n" + << hessian << "\n perturbed_hessian:\n" + << peturbed_hessian << "\n difference:\n" + << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference( + q_vec, q_vec + epsilon2); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2)) + << "\n hessian:\n" + << hessian << "\n perturbed_hessian:\n" + << peturbed_hessian << "\n difference:\n" + << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference( + q_vec, q_vec + epsilon3); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2)) + << "\n hessian:\n" + << hessian << "\n perturbed_hessian:\n" + << peturbed_hessian << "\n difference:\n" + << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference( + q_vec, q_vec + epsilon4); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2)) + << "\n hessian:\n" + << hessian << "\n perturbed_hessian:\n" + << peturbed_hessian << "\n difference:\n" + << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference( + q_vec, q_vec + epsilon1 + epsilon2); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2)) + << "\n hessian:\n" + << hessian << "\n perturbed_hessian:\n" + << peturbed_hessian << "\n difference:\n" + << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference( + q_vec, q_vec + epsilon2 + epsilon3); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2)) + << "\n hessian:\n" + << hessian << "\n perturbed_hessian:\n" + << peturbed_hessian << "\n difference:\n" + << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference( + q_vec, q_vec + epsilon3 + epsilon4); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2)) + << "\n hessian:\n" + << hessian << "\n perturbed_hessian:\n" + << peturbed_hessian << "\n difference:\n" + << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference( + q_vec, q_vec + epsilon4 + epsilon1); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2)) + << "\n hessian:\n" + << hessian << "\n perturbed_hessian:\n" + << peturbed_hessian << "\n difference:\n" + << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference( + q_vec, q_vec + epsilon3 + epsilon1); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2)) + << "\n hessian:\n" + << hessian << "\n perturbed_hessian:\n" + << peturbed_hessian << "\n difference:\n" + << (hessian - peturbed_hessian); + + peturbed_hessian = common::hessian_of_squared_quaternion_angle_difference( + q_vec, q_vec + epsilon4 + epsilon2); + EXPECT_TRUE(hessian.isApprox(peturbed_hessian, 1e-2)) + << "\n hessian:\n" + << hessian << "\n perturbed_hessian:\n" + << peturbed_hessian << "\n difference:\n" + << (hessian - peturbed_hessian); } } -} // namespace test -} // namespace systems -} // namespace c3 \ No newline at end of file +} // namespace test +} // namespace systems +} // namespace c3 diff --git a/systems/test/quaternion_cost_test.yaml b/systems/test/quaternion_cost_test.yaml index cce4e0f..d3c1dd1 100644 --- a/systems/test/quaternion_cost_test.yaml +++ b/systems/test/quaternion_cost_test.yaml @@ -8,6 +8,7 @@ publish_frequency: 100 state_prediction_joints: [] quaternion_indices: [5] +#leave empty to disable any quaternion-dependent cost weights quaternion_weight: 1000 quaternion_regularizer_fraction: 0.0