Skip to content

Commit 6b42f54

Browse files
astomodynamicsastomodynamics
committed
Tighten IPDDP terminal constraint handling
1 parent f5ba105 commit 6b42f54

3 files changed

Lines changed: 153 additions & 7 deletions

File tree

‎include/cddp-cpp/cddp_core/options.hpp‎

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -150,9 +150,6 @@ namespace cddp
150150
double dual_var_init_scale = 1e-1; ///< Initial scale for dual variables.
151151
double slack_var_init_scale = 1e-2; ///< Initial scale for slack variables.
152152

153-
bool make_psd = true; ///< Project Q_uu to PSD cone before solve.
154-
double psd_delta = 1e-6; ///< Minimum eigenvalue for PSD projection.
155-
156153
double barrier_tol_mult =
157154
0.1; ///< Barrier-scaled inner tolerance multiplier.
158155
double barrier_update_dual_weight =
@@ -164,7 +161,7 @@ namespace cddp
164161
std::string theta_norm =
165162
"l1"; ///< Constraint violation norm used by the filter ("l1" or "l2").
166163
int max_filter_size =
167-
50; ///< Maximum number of active non-dominated filter entries.
164+
5; ///< Maximum number of active non-dominated filter entries.
168165
double theta_0_floor =
169166
1.0; ///< Minimum theta_0 value used for filter initialization.
170167

‎src/cddp_core/ipddp_solver.cpp‎

Lines changed: 93 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -889,6 +889,78 @@ namespace cddp
889889
k_lambda_.back() = V_x;
890890
K_lambda_.back() = V_xx;
891891

892+
if (!has_path_constraints && !has_terminal_ineq && has_terminal_eq)
893+
{
894+
const Eigen::VectorXd h_T =
895+
evaluateTerminalEqualityResidual(context, context.X_.back());
896+
const Eigen::MatrixXd H_T =
897+
evaluateTerminalEqualityJacobian(context, context.X_.back());
898+
899+
std::vector<Eigen::MatrixXd> Q(horizon + 1,
900+
Eigen::MatrixXd::Zero(state_dim, state_dim));
901+
std::vector<Eigen::VectorXd> q(horizon + 1,
902+
Eigen::VectorXd::Zero(state_dim));
903+
std::vector<Eigen::MatrixXd> R(horizon,
904+
Eigen::MatrixXd::Zero(control_dim, control_dim));
905+
std::vector<Eigen::VectorXd> r(horizon,
906+
Eigen::VectorXd::Zero(control_dim));
907+
std::vector<Eigen::MatrixXd> M(horizon,
908+
Eigen::MatrixXd::Zero(state_dim, control_dim));
909+
std::vector<Eigen::MatrixXd> A_vec(horizon,
910+
Eigen::MatrixXd::Zero(state_dim, state_dim));
911+
std::vector<Eigen::MatrixXd> B_vec(horizon,
912+
Eigen::MatrixXd::Zero(state_dim, control_dim));
913+
std::vector<Eigen::VectorXd> d_vec(horizon,
914+
Eigen::VectorXd::Zero(state_dim));
915+
916+
Q.back() = V_xx;
917+
q.back() = context.getObjective().getFinalCostGradient(context.X_.back());
918+
919+
for (int t = 0; t < horizon; ++t)
920+
{
921+
auto [l_x, l_u] =
922+
context.getObjective().getRunningCostGradients(context.X_[t], context.U_[t], t);
923+
auto [l_xx, l_uu, l_ux] =
924+
context.getObjective().getRunningCostHessians(context.X_[t], context.U_[t], t);
925+
926+
Q[t] = 0.5 * (l_xx + l_xx.transpose());
927+
q[t] = l_x;
928+
R[t] = 0.5 * (l_uu + l_uu.transpose());
929+
R[t].diagonal().array() += context.regularization_;
930+
r[t] = l_u;
931+
M[t] = l_ux.transpose();
932+
A_vec[t] = F_x_[t];
933+
B_vec[t] = F_u_[t];
934+
}
935+
936+
Eigen::VectorXd lambda_total;
937+
Eigen::VectorXd lambda_delta;
938+
if (!solveTerminalEqualityLQR(
939+
Q, q, R, r, M, A_vec, B_vec, d_vec,
940+
Eigen::VectorXd::Zero(state_dim), H_T, -h_T, mu_,
941+
options.ipddp.jacobian_regularization_value,
942+
options.ipddp.jacobian_regularization_exponent, &Lambda_T_eq_,
943+
K_u_, k_u_, K_lambda_, k_lambda_, lambda_total, lambda_delta))
944+
{
945+
return false;
946+
}
947+
948+
dLambda_T_eq_ = lambda_delta;
949+
dV_ = Eigen::Vector2d::Zero();
950+
inf_du = lambda_delta.lpNorm<Eigen::Infinity>();
951+
for (int t = 0; t < horizon; ++t)
952+
{
953+
inf_du = std::max(inf_du, k_u_[t].lpNorm<Eigen::Infinity>());
954+
step_norm = std::max(step_norm, k_u_[t].lpNorm<Eigen::Infinity>());
955+
}
956+
957+
context.inf_du_ = inf_du;
958+
context.step_norm_ = step_norm;
959+
context.inf_pr_ = h_T.lpNorm<Eigen::Infinity>();
960+
context.inf_comp_ = 0.0;
961+
return true;
962+
}
963+
892964
if (!has_path_constraints && !has_terminal_ineq && !has_terminal_eq)
893965
{
894966
for (int t = horizon - 1; t >= 0; --t)
@@ -1388,7 +1460,7 @@ namespace cddp
13881460
}
13891461

13901462
bool filter_acceptance = false;
1391-
if (constraint_set.empty() && !has_terminal_ineq)
1463+
if (constraint_set.empty() && !has_terminal_ineq && !has_terminal_eq)
13921464
{
13931465
double dJ = context.cost_ - cost_new;
13941466
double expected = -alpha_pr * (dV_(0) + 0.5 * alpha_pr * dV_(1));
@@ -2033,6 +2105,10 @@ namespace cddp
20332105
has_terminal_eq ? &h_T : nullptr),
20342106
std::max(context.getOptions().ipddp.theta_0_floor, 1e-8));
20352107
filter_.clear();
2108+
if (has_terminal_ineq || has_terminal_eq)
2109+
{
2110+
acceptFilterEntry(phi_, theta_);
2111+
}
20362112
}
20372113

20382114
void IPDDPSolver::resetFilter(CDDP &context) { resetBarrierFilter(context); }
@@ -2136,11 +2212,16 @@ namespace cddp
21362212
if (reset_filter)
21372213
{
21382214
filter_.clear();
2215+
if (getTerminalEqualityDim(context) > 0 ||
2216+
!getTerminalInequalityLayout(context).empty())
2217+
{
2218+
acceptFilterEntry(phi_, theta_);
2219+
}
21392220
}
21402221
else
21412222
{
21422223
acceptFilterEntry(phi_, theta_);
2143-
if (static_cast<int>(filter_.size()) > 5)
2224+
if (static_cast<int>(filter_.size()) > options.ipddp.max_filter_size)
21442225
{
21452226
detail::pruneFilterToBestPoints(filter_);
21462227
}
@@ -2221,7 +2302,16 @@ namespace cddp
22212302
double IPDDPSolver::computeMaxConstraintViolation(const CDDP &context) const
22222303
{
22232304
(void)context;
2224-
return detail::computeMaxConstraintViolation(G_);
2305+
double max_violation = detail::computeMaxConstraintViolation(G_);
2306+
for (const auto &entry : G_T_)
2307+
{
2308+
if (entry.second.size() == 0)
2309+
{
2310+
continue;
2311+
}
2312+
max_violation = std::max(max_violation, entry.second.maxCoeff());
2313+
}
2314+
return max_violation;
22252315
}
22262316

22272317
double IPDDPSolver::computeScaledDualInfeasibility(const CDDP &context) const

‎tests/cddp_core/test_ipddp_solver.cpp‎

Lines changed: 59 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -883,3 +883,62 @@ TEST(IPDDPTest, SolveWithTerminalInequalityOnly)
883883
EXPECT_LE(solution.state_trajectory.back()(0), 1e-4);
884884
EXPECT_LT(solution.state_trajectory.back()(0), goal_state(0));
885885
}
886+
887+
TEST(IPDDPTest, SolveWithTerminalEqualityOnly)
888+
{
889+
const int state_dim = 1;
890+
const int control_dim = 1;
891+
const int horizon = 8;
892+
const double timestep = 1.0;
893+
894+
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(state_dim, state_dim);
895+
Eigen::MatrixXd B = Eigen::MatrixXd::Identity(state_dim, control_dim);
896+
897+
Eigen::VectorXd initial_state(state_dim);
898+
initial_state << 1.0;
899+
Eigen::VectorXd goal_state(state_dim);
900+
goal_state << 0.0;
901+
902+
cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep);
903+
cddp_solver.setDynamicalSystem(
904+
std::make_unique<cddp::LTISystem>(A, B, timestep));
905+
906+
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(state_dim, state_dim);
907+
Eigen::MatrixXd R = 1e-2 * Eigen::MatrixXd::Identity(control_dim, control_dim);
908+
Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim);
909+
cddp_solver.setObjective(std::make_unique<cddp::QuadraticObjective>(
910+
Q, R, Qf, goal_state, std::vector<Eigen::VectorXd>(), timestep));
911+
912+
cddp::CDDPOptions options;
913+
options.max_iterations = 100;
914+
options.tolerance = 1e-6;
915+
options.acceptable_tolerance = 1e-6;
916+
options.enable_parallel = false;
917+
options.num_threads = 1;
918+
options.verbose = false;
919+
options.debug = false;
920+
options.regularization.initial_value = 1e-6;
921+
cddp_solver.setOptions(options);
922+
923+
cddp_solver.addTerminalConstraint(
924+
"TerminalTarget",
925+
std::make_unique<cddp::TerminalEqualityConstraint>(goal_state));
926+
927+
std::vector<Eigen::VectorXd> X(horizon + 1, Eigen::VectorXd::Zero(state_dim));
928+
std::vector<Eigen::VectorXd> U(horizon, Eigen::VectorXd::Zero(control_dim));
929+
X[0] = initial_state;
930+
for (int t = 0; t < horizon; ++t)
931+
{
932+
X[t + 1] = A * X[t] + B * U[t];
933+
}
934+
cddp_solver.setInitialTrajectory(X, U);
935+
936+
const cddp::CDDPSolution solution = cddp_solver.solve("IPDDP");
937+
938+
EXPECT_TRUE(solution.status_message == "OptimalSolutionFound" ||
939+
solution.status_message == "AcceptableSolutionFound");
940+
ASSERT_FALSE(solution.state_trajectory.empty());
941+
const double terminal_residual =
942+
(solution.state_trajectory.back() - goal_state).lpNorm<Eigen::Infinity>();
943+
EXPECT_LE(terminal_residual, 1e-4);
944+
}

0 commit comments

Comments
 (0)