diff --git a/results/tests/dubincs_car_logcddp_test.png b/results/tests/dubincs_car_logcddp_test.png index 9f22c4a..1a25da1 100644 Binary files a/results/tests/dubincs_car_logcddp_test.png and b/results/tests/dubincs_car_logcddp_test.png differ diff --git a/src/cddp_core/clddp_core.cpp b/src/cddp_core/clddp_core.cpp index d27962e..63d00c5 100644 --- a/src/cddp_core/clddp_core.cpp +++ b/src/cddp_core/clddp_core.cpp @@ -268,24 +268,24 @@ bool CDDP::solveCLDDPBackwardPass() { k = -H * Q_u; K = -H * Q_ux; } else { + // TODO: Migrate to BoxQP solver + // // Solve Box QP Problem using BoxQPSolver + // Eigen::VectorXd lower = control_box_constraint->getLowerBound() - u; + // Eigen::VectorXd upper = control_box_constraint->getUpperBound() - u; - // Solve Box QP Problem using BoxQPSolver - Eigen::VectorXd lower = control_box_constraint->getLowerBound() - u; - Eigen::VectorXd upper = control_box_constraint->getUpperBound() - u; + // cddp::BoxQPResult qp_result = qp_solver.solve(Q_uu, Q_u, lower, upper, u); - cddp::BoxQPResult qp_result = qp_solver.solve(Q_uu, Q_u, lower, upper, u); + // // if (qp_result.status != cddp::BoxQPStatus::SMALL_GRADIENT && + // // qp_result.status != cddp::BoxQPStatus::SMALL_IMPROVEMENT) { + // // std::cout << "BoxQP solver failed with status: " << static_cast(qp_result.status) << std::endl; + // // return false; + // // } - // if (qp_result.status != cddp::BoxQPStatus::SMALL_GRADIENT && - // qp_result.status != cddp::BoxQPStatus::SMALL_IMPROVEMENT) { - // std::cout << "BoxQP solver failed with status: " << static_cast(qp_result.status) << std::endl; - // return false; - // } - - // Extract solution - k = qp_result.x; // Feedforward term + // // Extract solution + // k = qp_result.x; // Feedforward term - const auto &H = qp_result.Hfree; - const auto &free = qp_result.free; + // const auto &H = qp_result.Hfree; + // const auto &free = qp_result.free; // const Eigen::MatrixXd &Q_ux_free = Q_ux(free, Eigen::all); @@ -299,33 +299,33 @@ bool CDDP::solveCLDDPBackwardPass() { /* Solve Box QP Problem */ - // int numNonZeros = Q_uu.nonZeros(); - // P.reserve(numNonZeros); - // P.setZero(); - // for (int i = 0; i < Q_uu.rows(); ++i) { - // for (int j = 0; j < Q_uu.cols(); ++j) { - // if (Q_uu(i, j) != 0) { - // P.insert(i, j) = Q_uu(i, j); - // } - // } - // } - // P.makeCompressed(); // Important for efficient storage and operations - // osqp_solver_.UpdateObjectiveMatrix(P); - - // const Eigen::VectorXd& q = Q_u; // Gradient of QP objective - // osqp_solver_.SetObjectiveVector(q); - - // // Lower and upper bounds - // Eigen::VectorXd lb = 1.0 * (control_box_constraint->getLowerBound() - u); - // Eigen::VectorXd ub = 1.0 * (control_box_constraint->getUpperBound() - u); - // osqp_solver_.SetBounds(lb, ub); - - // // Solve the QP problem TODO: Use SDQP instead of OSQP - // osqp::OsqpExitCode exit_code = osqp_solver_.Solve(); + int numNonZeros = Q_uu.nonZeros(); + P.reserve(numNonZeros); + P.setZero(); + for (int i = 0; i < Q_uu.rows(); ++i) { + for (int j = 0; j < Q_uu.cols(); ++j) { + if (Q_uu(i, j) != 0) { + P.insert(i, j) = Q_uu(i, j); + } + } + } + P.makeCompressed(); // Important for efficient storage and operations + osqp_solver_.UpdateObjectiveMatrix(P); - // // Extract solution - // double optimal_objective = osqp_solver_.objective_value(); - // k = osqp_solver_.primal_solution(); + const Eigen::VectorXd& q = Q_u; // Gradient of QP objective + osqp_solver_.SetObjectiveVector(q); + + // Lower and upper bounds + Eigen::VectorXd lb = 1.0 * (control_box_constraint->getLowerBound() - u); + Eigen::VectorXd ub = 1.0 * (control_box_constraint->getUpperBound() - u); + osqp_solver_.SetBounds(lb, ub); + + // Solve the QP problem TODO: Use SDQP instead of OSQP + osqp::OsqpExitCode exit_code = osqp_solver_.Solve(); + + // Extract solution + double optimal_objective = osqp_solver_.objective_value(); + k = osqp_solver_.primal_solution(); // // Compute gain matrix K = -Q_uu.inverse() * Q_ux; diff --git a/src/cddp_core/logddp_core.cpp b/src/cddp_core/logddp_core.cpp index a446a40..d0357d5 100644 --- a/src/cddp_core/logddp_core.cpp +++ b/src/cddp_core/logddp_core.cpp @@ -323,13 +323,9 @@ bool CDDP::solveLogCDDPForwardPass() Eigen::VectorXd du = alpha * k_[t] + K_[t] * dx; U_new[t] = u + du; - // Check constraints (using strict inequalities) - for (int i = 0; i < control_dim; ++i) { - if (U_new[t](i) < control_box_constraint->getLowerBound()(i) || - U_new[t](i) > control_box_constraint->getUpperBound()(i)) { - is_feasible = false; - break; - } + // Clamp control input + if (control_box_constraint != nullptr) { + U_new[t] = control_box_constraint->clamp(U_new[t]); } if (!is_feasible) { diff --git a/tests/cddp_core/test_logcddp_core.cpp b/tests/cddp_core/test_logcddp_core.cpp index 99743af..f74edb2 100644 --- a/tests/cddp_core/test_logcddp_core.cpp +++ b/tests/cddp_core/test_logcddp_core.cpp @@ -91,42 +91,42 @@ TEST(CDDPTest, SolveLogCDDP) { auto U_sol = solution.control_sequence; // size: horizon auto t_sol = solution.time_sequence; // size: horizon + 1 -// // Create directory for saving plot (if it doesn't exist) -// const std::string plotDirectory = "../results/tests"; -// if (!fs::exists(plotDirectory)) { -// fs::create_directory(plotDirectory); -// } - -// // Plot the solution (x-y plane) -// std::vector x_arr, y_arr, theta_arr; -// for (const auto& x : X_sol) { -// x_arr.push_back(x(0)); -// y_arr.push_back(x(1)); -// theta_arr.push_back(x(2)); -// } - -// // Plot the solution (control inputs) -// std::vector v_arr, omega_arr; -// for (const auto& u : U_sol) { -// v_arr.push_back(u(0)); -// omega_arr.push_back(u(1)); -// } - -// // Plot the solution by subplots -// plt::subplot(2, 1, 1); -// plt::plot(x_arr, y_arr); -// plt::title("State Trajectory"); -// plt::xlabel("x"); -// plt::ylabel("y"); - -// plt::subplot(2, 1, 2); -// plt::plot(v_arr); -// plt::plot(omega_arr); -// // Plot boundaries -// plt::plot(std::vector(U_sol.size(), -1.0), "r--"); -// plt::plot(std::vector(U_sol.size(), 1.0), "r--"); -// plt::title("Control Inputs"); -// plt::save(plotDirectory + "/dubincs_car_logcddp_test.png"); + // // Create directory for saving plot (if it doesn't exist) + // const std::string plotDirectory = "../results/tests"; + // if (!fs::exists(plotDirectory)) { + // fs::create_directory(plotDirectory); + // } + + // // Plot the solution (x-y plane) + // std::vector x_arr, y_arr, theta_arr; + // for (const auto& x : X_sol) { + // x_arr.push_back(x(0)); + // y_arr.push_back(x(1)); + // theta_arr.push_back(x(2)); + // } + + // // Plot the solution (control inputs) + // std::vector v_arr, omega_arr; + // for (const auto& u : U_sol) { + // v_arr.push_back(u(0)); + // omega_arr.push_back(u(1)); + // } + + // // Plot the solution by subplots + // plt::subplot(2, 1, 1); + // plt::plot(x_arr, y_arr); + // plt::title("State Trajectory"); + // plt::xlabel("x"); + // plt::ylabel("y"); + + // plt::subplot(2, 1, 2); + // plt::plot(v_arr); + // plt::plot(omega_arr); + // // Plot boundaries + // plt::plot(std::vector(U_sol.size(), -1.0), "r--"); + // plt::plot(std::vector(U_sol.size(), 1.0), "r--"); + // plt::title("Control Inputs"); + // plt::save(plotDirectory + "/dubincs_car_logcddp_test.png"); // // Create figure and axes // plt::figure_size(800, 600);