diff --git a/bindings/pyc3/c3_systems_py.cc b/bindings/pyc3/c3_systems_py.cc index cc12670..ebeb288 100644 --- a/bindings/pyc3/c3_systems_py.cc +++ b/bindings/pyc3/c3_systems_py.cc @@ -175,10 +175,14 @@ PYBIND11_MODULE(systems, m) { .def_readwrite("lcs_factory_options", &C3ControllerOptions::lcs_factory_options) .def_readwrite("state_prediction_joints", - &C3ControllerOptions::state_prediction_joints); + &C3ControllerOptions::state_prediction_joints) + .def_readwrite("quaternion_weight", + &C3ControllerOptions::quaternion_weight) + .def_readwrite("quaternion_regularizer_fraction", + &C3ControllerOptions::quaternion_regularizer_fraction); m.def("LoadC3ControllerOptions", &LoadC3ControllerOptions); } } // namespace pyc3 } // namespace systems -} // namespace c3 \ 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..00d56b8 100644 --- a/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml +++ b/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml @@ -7,6 +7,10 @@ state_prediction_joints: [] # - name : "CartSlider" # max_acceleration : 10 +#leave empty to disable any quaternion-dependent cost weights +quaternion_weight: 0.0 +quaternion_regularizer_fraction: 0.0 + lcs_factory_options: #options are 'stewart_and_trinkle' or 'anitescu' # contact_model : 'stewart_and_trinkle' diff --git a/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml b/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml index 50b49bf..b14d696 100644 --- a/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml +++ b/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml @@ -5,6 +5,10 @@ solve_time_filter_alpha: 0.0 publish_frequency: 100 state_prediction_joints: [] +#leave empty to disable any quaternion-dependent cost weights +quaternion_weight: 0.0 +quaternion_regularizer_fraction: 0.0 + lcs_factory_options: #options are 'stewart_and_trinkle' or 'anitescu' # contact_model : 'stewart_and_trinkle' 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..54fe405 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,25 @@ cc_test( ":framework", "@gtest//:main", ], + size = "small", +) + +cc_test( + name = "quaternion_cost_test", + srcs = [ + "test/quaternion_cost_test.cc" + ], + data = [ + "test/resources/cube.sdf", + "test/resources/plate.sdf", + "test/resources/quaternion_cost_test.yaml", + ], + deps = [ + "//core:c3", + ":systems", + "@gtest//:main", + ], + size = "small", ) filegroup( diff --git a/systems/c3_controller.cc b/systems/c3_controller.cc index 85d4315..d3edff4 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,9 @@ drake::systems::EventStatus C3Controller::ComputePlan( if (!state_prediction_joints_.empty()) ResolvePredictedState(x0, filtered_solve_time); + // Update cost matrices with quaternion cost blocks + UpdateQuaternionCosts(x0, desired_state.value()); + // Update LCS and target in the C3 problem c3_->UpdateLCS(lcs); c3_->UpdateTarget(target); @@ -272,5 +276,66 @@ void C3Controller::OutputC3Intermediates( } } +void C3Controller::UpdateQuaternionCosts(const Eigen::VectorXd& x_curr, + const Eigen::VectorXd& x_des) const { + // Extract quaternion indices + vector quaternion_indices; + for (const auto& body_idx : plant_.GetFloatingBaseBodies()) { + const auto& body = plant_.get_body(body_idx); + int start = body.floating_positions_start(); + quaternion_indices.push_back(start); + } + + // Early return if no quaternions or cost parameters not set + if (quaternion_indices.size() == 0 || + !controller_options_.quaternion_weight.has_value() || + !controller_options_.quaternion_regularizer_fraction.has_value()) { + return; + } + + C3::CostMatrices costs = c3_->GetCostMatrices(); + vector Q = costs.Q; + vector R = costs.R; + vector G = costs.G; + vector U = costs.U; + + for (int index : quaternion_indices) { + Eigen::VectorXd quat_curr_i = x_curr.segment(index, 4); + Eigen::VectorXd quat_des_i = x_des.segment(index, 4); + + Eigen::MatrixXd quat_hessian_i = + common::hessian_of_squared_quaternion_angle_difference(quat_curr_i, + quat_des_i); + + // Regularize hessian so Q is always PSD + double min_eigenval = quat_hessian_i.eigenvalues().real().minCoeff(); + Eigen::MatrixXd quat_regularizer_1 = + std::max(0.0, -min_eigenval) * Eigen::MatrixXd::Identity(4, 4); + Eigen::MatrixXd quat_regularizer_2 = quat_des_i * quat_des_i.transpose(); + + // Additional regularization term to help with numerical issues + Eigen::MatrixXd quat_regularizer_3 = 1e-8 * Eigen::MatrixXd::Identity(4, 4); + + double quaternion_weight = controller_options_.quaternion_weight.value(); + double quaternion_regularizer_fraction = + controller_options_.quaternion_regularizer_fraction.value(); + + // Replace quaternion blocks in Q + double discount_factor = 1; + for (int i = 0; i < N_ + 1; i++) { + Q[i].block(index, index, 4, 4) = + discount_factor * quaternion_weight * + (quat_hessian_i + quat_regularizer_1 + + quaternion_regularizer_fraction * quat_regularizer_2 + + quat_regularizer_3); + discount_factor *= controller_options_.c3_options.gamma; + } + } + + // Set C3 cost matrices + C3::CostMatrices new_costs(Q, R, G, U); + c3_->UpdateCostMatrices(new_costs); +} + } // namespace systems } // namespace c3 diff --git a/systems/c3_controller.h b/systems/c3_controller.h index 8a8df72..1fff472 100644 --- a/systems/c3_controller.h +++ b/systems/c3_controller.h @@ -71,6 +71,21 @@ class C3Controller : public drake::systems::LeafSystem { c3_->UpdateCostMatrices(costs); } + /** + * @brief Returns the cost matrices used by the controller. + * @return The cost matrices to being used in the optimization problem. + */ + const C3::CostMatrices& GetCostMatrices() { return c3_->GetCostMatrices(); } + + /** + * @brief Updates any 4x4 portions of the cost weight matrix corresponding to + * quaternion states that are set to have quaternion-dependent costs. + * @param x_curr The current state vector. + * @param x_des The desired state vector. + */ + void UpdateQuaternionCosts(const VectorXd& x_curr, + const Eigen::VectorXd& x_des) const; + /** * @brief Adds a linear constraint to the controller. * @param A The constraint matrix. @@ -214,10 +229,11 @@ class C3Controller : public drake::systems::LeafSystem { 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..179fa67 100644 --- a/systems/c3_controller_options.h +++ b/systems/c3_controller_options.h @@ -60,6 +60,11 @@ struct C3ControllerOptions { double solve_time_filter_alpha = 0.0; double publish_frequency = 100.0; // Hz + std::optional quaternion_weight = + 0.0; // Quaternion cost scaling term + std::optional quaternion_regularizer_fraction = + 0.0; // Outer product regularization scaling term + std::vector state_prediction_joints; template @@ -70,6 +75,8 @@ struct C3ControllerOptions { a->Visit(DRAKE_NVP(solve_time_filter_alpha)); a->Visit(DRAKE_NVP(publish_frequency)); a->Visit(DRAKE_NVP(state_prediction_joints)); + a->Visit(DRAKE_NVP(quaternion_weight)); + a->Visit(DRAKE_NVP(quaternion_regularizer_fraction)); if (projection_type == "QP") { DRAKE_DEMAND(lcs_factory_options.contact_model == "anitescu"); 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..5a0f9b1 --- /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 (w, x, y, z). The Hessian of the squared + * angle difference is with respect to this quaternion. + * @param quat_desired The second quaternion (w, x, y, z). + * @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..df1a0cd --- /dev/null +++ b/systems/test/quaternion_cost_test.cc @@ -0,0 +1,238 @@ +#include +#include +#include + +#include +#include +#include + +#include "core/c3.h" +#include "systems/c3_controller.h" +#include "systems/c3_controller_options.h" +#include "systems/common/quaternion_error_hessian.h" + +using c3::systems::C3Controller; +using c3::systems::C3ControllerOptions; +using drake::multibody::AddMultibodyPlantSceneGraph; +using drake::multibody::MultibodyPlant; +using drake::multibody::Parser; +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: + C3ControllerOptions controller_options_; + std::unique_ptr> diagram_; + C3Controller* c3_controller_; + + void SetUp() override { + DiagramBuilder builder; + auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(&builder, 0); + Parser parser(&plant, &scene_graph); + parser.AddModels("systems/test/resources/cube.sdf"); + parser.AddModels("systems/test/resources/plate.sdf"); + plant.Finalize(); + + controller_options_ = drake::yaml::LoadYamlFile( + "systems/test/resources/quaternion_cost_test.yaml"); + + vector Q_dummy; + vector R_dummy; + vector G_dummy; + vector U_dummy; + + double discount_factor = 1; + for (int i = 0; i < controller_options_.lcs_factory_options.N; i++) { + Q_dummy.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_dummy.push_back(discount_factor * controller_options_.c3_options.R); + G_dummy.push_back(controller_options_.c3_options.G); + U_dummy.push_back(controller_options_.c3_options.U); + } + } + Q_dummy.push_back(discount_factor * controller_options_.c3_options.Q); + + C3::CostMatrices dummy_costs = + C3::CostMatrices(Q_dummy, R_dummy, G_dummy, U_dummy); + + c3_controller_ = builder.AddSystem(plant, dummy_costs, + controller_options_); + diagram_ = builder.Build(); + } +}; + +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); + + 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); + + 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) { + VectorXd q1(4); + q1 << 0.5, 0.5, 0.5, 0.5; + VectorXd q2(4); + q2 << 0.707, 0, 0, 0.707; + + c3_controller_->UpdateQuaternionCosts(q1, q2); + + C3::CostMatrices output = c3_controller_->GetCostMatrices(); + vector Q = output.Q; + + 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; + start_indices.push_back(5); // Index of quaternion + + 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/resources/cube.sdf b/systems/test/resources/cube.sdf new file mode 100644 index 0000000..1957084 --- /dev/null +++ b/systems/test/resources/cube.sdf @@ -0,0 +1,185 @@ + + + + + + 0 0 0 0 0 0 + + 0.05 + + 0.000033 + 0 + 0 + 0.000033 + 0 + 0.0000625 + + + + + + + 0.05 + 0.02 + + + 0.8 0.4 0.0 1 + + + 0 0 0.1 0 0 0 + + + 0.01 + 0.2 + + + + 0 0 1 1 + 0 0 1 1 + + + + + + 0.1 0 0 0 1.57079632679 0 + + + 0.01 + 0.2 + + + + 1 0 0 1 + 1 0 0 1 + + + + + + 0 0.1 0 1.57079632679 0 0 + + + 0.01 + 0.2 + + + + 0 1 0 1 + 0 1 0 1 + + + + + 0 0 0 0 0 0 + + + 0.05 + 0.02 + + + + 0.6 + 0.5 + + + + + 0.05 0 0 0 0 0 + + + 0.001 0.001 0.001 + + + + 0.5 + 0.4 + + + + -0.05 0 0 0 0 0 + + + 0.001 0.001 0.001 + + + + 0.5 + 0.4 + + + + 0 0.05 0 0 0 0 + + + 0.001 0.001 0.001 + + + + 0.5 + 0.4 + + + + 0 -0.05 0 0 0 0 + + + 0.001 0.001 0.001 + + + + 0.5 + 0.4 + + + + 0.03535 -0.03535 0 0 0 0 + + + 0.001 0.001 0.001 + + + + 0.5 + 0.4 + + + + -0.03535 -0.03535 0 0 0 0 + + + 0.001 0.001 0.001 + + + + 0.5 + 0.4 + + + + -0.03535 0.03535 0 0 0 0 + + + 0.001 0.001 0.001 + + + + 0.5 + 0.4 + + + + 0.03535 0.03535 0 0 0 0 + + + 0.001 0.001 0.001 + + + + 0.5 + 0.4 + + + + + + diff --git a/systems/test/resources/plate.sdf b/systems/test/resources/plate.sdf new file mode 100644 index 0000000..f62ae62 --- /dev/null +++ b/systems/test/resources/plate.sdf @@ -0,0 +1,162 @@ + + + + + + + + + 0 0 0 0 0 0 + + 0.5 + + 0.005 + 0 + 0 + 0.005 + 0 + 0.01 + + + + + + 0.2 + 0.02 + + + 0.1 0.1 0.1 1 + + + + + 0.2 + 0.02 + + + + 0.6 + 0.5 + + + + + + + + + 1.0e-6 + + 1.0e-6 + 0 + 0 + 1.0e-6 + 0 + 1.0e-6 + + + + + world + link_x + + 1 0 0 + + + + + + + 1.0e-6 + + 1.0e-6 + 0 + 0 + 1.0e-6 + 0 + 1.0e-6 + + + + + link_x + link_y + + 0 1 0 + + + + + + + 1.0e-6 + + 1.0e-6 + 0 + 0 + 1.0e-6 + 0 + 1.0e-6 + + + + + link_y + link_z + + 0 0 1 + + + + + + + 1.0e-6 + + 1.0e-6 + 0 + 0 + 1.0e-6 + 0 + 1.0e-6 + + + + + link_z + link_roll + + 1 0 0 + + + + + + link_roll + plate + 0 0 0 0 0 0 + + 0 1 0 + + + + + + + + diff --git a/systems/test/resources/quaternion_cost_test.yaml b/systems/test/resources/quaternion_cost_test.yaml new file mode 100644 index 0000000..dfaeeb0 --- /dev/null +++ b/systems/test/resources/quaternion_cost_test.yaml @@ -0,0 +1,70 @@ +# 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_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]