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/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/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/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 29eb400..94471c8 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", @@ -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( diff --git a/systems/c3_controller.cc b/systems/c3_controller.cc index 85d4315..acaf3ae 100644 --- a/systems/c3_controller.cc +++ b/systems/c3_controller.cc @@ -4,6 +4,7 @@ #include +#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 diff --git a/systems/c3_controller.h b/systems/c3_controller.h index 8a8df72..d271c9b 100644 --- a/systems/c3_controller.h +++ b/systems/c3_controller.h @@ -179,6 +179,15 @@ class C3Controller : public drake::systems::LeafSystem { void OutputC3Intermediates(const drake::systems::Context& 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 { 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..9a761f4 100644 --- a/systems/c3_controller_options.h +++ b/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 + 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 +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"); diff --git a/systems/common/quaternion_error_hessian.cc b/systems/common/quaternion_error_hessian.cc new file mode 100644 index 0000000..9f8d03d --- /dev/null +++ b/systems/common/quaternion_error_hessian.cc @@ -0,0 +1,196 @@ +#include "quaternion_error_hessian.h" + +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 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 + + // 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); + + // 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); + + 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 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; + + return hessian; +} + +} // namespace common +} // namespace systems +} // namespace c3 diff --git a/systems/common/quaternion_error_hessian.h b/systems/common/quaternion_error_hessian.h new file mode 100644 index 0000000..5f99399 --- /dev/null +++ b/systems/common/quaternion_error_hessian.h @@ -0,0 +1,26 @@ +#include +#include + +#include "drake/common/drake_assert.h" + +using Eigen::MatrixXd; +using Eigen::VectorXd; + +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. + */ +MatrixXd hessian_of_squared_quaternion_angle_difference( + const VectorXd& quat, const VectorXd& quat_desired); + +} // namespace common +} // namespace systems +} // namespace c3 diff --git a/systems/test/quaternion_cost_test.cc b/systems/test/quaternion_cost_test.cc new file mode 100644 index 0000000..0c1d2d3 --- /dev/null +++ b/systems/test/quaternion_cost_test.cc @@ -0,0 +1,247 @@ +#include +#include +#include + +#include +#include + +#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; + +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); + 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(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); + 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))); + } + } +} +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 diff --git a/systems/test/quaternion_cost_test.yaml b/systems/test/quaternion_cost_test.yaml new file mode 100644 index 0000000..d3c1dd1 --- /dev/null +++ b/systems/test/quaternion_cost_test.yaml @@ -0,0 +1,72 @@ +# yaml parameter file for testing quaternion error Hessian computations + +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] +#leave empty to disable any quaternion-dependent cost weights +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]