Skip to content

Commit

Permalink
Fix CLDDP and LogDDP (#43)
Browse files Browse the repository at this point in the history 
* Fix solveCLDDPBackwardPass

* Fix solveLogCDDPForwardPass
  • Loading branch information
@astomodynamics
astomodynamics authored Dec 11, 2024
1 parent 9f5437c commit d7f1f8e
Show file tree
Hide file tree
Showing 4 changed files with 79 additions and 83 deletions.
Binary file modified results/tests/dubincs_car_logcddp_test.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
80 changes: 40 additions & 40 deletions src/cddp_core/clddp_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<int>(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<int>(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);

Expand 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;
Expand Down
10 changes: 3 additions & 7 deletions src/cddp_core/logddp_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
72 changes: 36 additions & 36 deletions tests/cddp_core/test_logcddp_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double> 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<double> 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<double>(U_sol.size(), -1.0), "r--");
// plt::plot(std::vector<double>(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<double> 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<double> 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<double>(U_sol.size(), -1.0), "r--");
// plt::plot(std::vector<double>(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);
Expand Down

0 comments on commit d7f1f8e

Please sign in to comment.