Skip to content

Commit e430d00

Browse files
astomodynamicsastomodynamics
committed
Add terminal constraint support to IPDDP
1 parent 0d7ab73 commit e430d00

7 files changed

Lines changed: 2260 additions & 1214 deletions

File tree

‎include/cddp-cpp/cddp.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
#include "cddp_core/dynamical_system.hpp"
2626
#include "cddp_core/objective.hpp"
2727
#include "cddp_core/constraint.hpp"
28+
#include "cddp_core/terminal_constraint.hpp"
2829
#include "cddp_core/barrier.hpp"
2930
#include "cddp_core/options.hpp"
3031
#include "cddp_core/cddp_core.hpp"

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

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -119,6 +119,9 @@ struct ForwardPassResult {
119119
bool success = false;
120120

121121
double constraint_violation = 0.0;
122+
double theta = 0.0; // Filter violation metric
123+
double inf_pr = 0.0; // Primal infeasibility
124+
double inf_comp = 0.0; // Complementary infeasibility
122125

123126
// Optional: Only relevant for certain solver strategies during their forward
124127
// pass

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

Lines changed: 71 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,9 @@ namespace cddp
3131
*
3232
* Inherits from CDDPSolverBase and overrides virtual hooks
3333
* for primal-dual interior point method with logarithmic barrier.
34+
* Supports path constraints, terminal equality constraints, and
35+
* terminal inequality constraints via a single-shooting formulation
36+
* with explicit costate variables.
3437
*/
3538
class IPDDPSolver : public CDDPSolverBase
3639
{
@@ -44,11 +47,15 @@ namespace cddp
4447
// === CDDPSolverBase virtual hook overrides ===
4548
void preIterationSetup(CDDP &context) override;
4649
bool backwardPass(CDDP &context) override;
50+
bool checkEarlyConvergence(CDDP &context, int iter,
51+
std::string &reason) override;
4752
ForwardPassResult forwardPass(CDDP &context, double alpha) override;
4853
void applyForwardPassResult(CDDP &context, const ForwardPassResult &result) override;
4954
bool checkConvergence(CDDP &context, double dJ, double dL, int iter,
5055
std::string &reason) override;
5156
void postIterationUpdate(CDDP &context, bool forward_pass_success) override;
57+
bool handleForwardPassFailure(CDDP &context,
58+
std::string &termination_reason) override;
5259
void recordIterationHistory(const CDDP &context) override;
5360
void populateSolverSpecificSolution(CDDPSolution &solution,
5461
const CDDP &context) override;
@@ -67,6 +74,7 @@ namespace cddp
6774
G_ux_; ///< Constraint mixed hessians
6875

6976
// Interior point variables
77+
std::map<std::string, std::vector<Eigen::VectorXd>> G_raw_; ///< Raw constraint values
7078
std::map<std::string, std::vector<Eigen::VectorXd>> G_; ///< Constraint values
7179
std::map<std::string, std::vector<Eigen::VectorXd>> Y_; ///< Dual variables
7280
std::map<std::string, std::vector<Eigen::VectorXd>> S_; ///< Slack variables
@@ -77,42 +85,40 @@ namespace cddp
7785
std::map<std::string, std::vector<Eigen::VectorXd>> k_s_; ///< Slack feedforward
7886
std::map<std::string, std::vector<Eigen::MatrixXd>> K_s_; ///< Slack feedback
7987

88+
// Single-shooting costate variables and gains
89+
std::vector<Eigen::VectorXd>
90+
Lambda_; ///< Costate variables (Lagrange multipliers for dynamics)
91+
std::vector<Eigen::VectorXd>
92+
k_lambda_; ///< Feedforward gains for costate variables
93+
std::vector<Eigen::MatrixXd>
94+
K_lambda_; ///< Feedback gains for costate variables
95+
96+
// Terminal equality constraint variables
97+
Eigen::VectorXd Lambda_T_eq_; ///< Terminal equality multipliers
98+
Eigen::VectorXd dLambda_T_eq_; ///< Terminal equality multiplier direction
99+
100+
// Terminal inequality constraint variables
101+
std::map<std::string, Eigen::VectorXd> G_T_; ///< Terminal inequality values
102+
std::map<std::string, Eigen::VectorXd> Y_T_; ///< Terminal inequality duals
103+
std::map<std::string, Eigen::VectorXd> S_T_; ///< Terminal inequality slacks
104+
std::map<std::string, Eigen::VectorXd> dY_T_; ///< Terminal inequality dual directions
105+
std::map<std::string, Eigen::VectorXd> dS_T_; ///< Terminal inequality slack directions
106+
107+
// Search directions
108+
std::vector<Eigen::VectorXd> dX_; ///< State search directions
109+
std::vector<Eigen::VectorXd> dU_; ///< Control search directions
110+
std::map<std::string, std::vector<Eigen::VectorXd>>
111+
dY_; ///< Dual search directions
112+
std::map<std::string, std::vector<Eigen::VectorXd>>
113+
dS_; ///< Slack search directions
114+
80115
// Barrier method parameters
81116
double mu_; ///< Barrier parameter
82117
std::vector<FilterPoint> filter_; ///< Filter for line search
83-
84-
// Pre-allocated workspace for performance optimization
85-
struct Workspace {
86-
// Backward pass workspace
87-
std::vector<Eigen::MatrixXd> A_matrices;
88-
std::vector<Eigen::MatrixXd> B_matrices;
89-
std::vector<Eigen::MatrixXd> Q_xx_matrices;
90-
std::vector<Eigen::MatrixXd> Q_ux_matrices;
91-
std::vector<Eigen::MatrixXd> Q_uu_matrices;
92-
std::vector<Eigen::VectorXd> Q_x_vectors;
93-
std::vector<Eigen::VectorXd> Q_u_vectors;
94-
95-
// LDLT solver cache
96-
std::vector<Eigen::LDLT<Eigen::MatrixXd>> ldlt_solvers;
97-
std::vector<bool> ldlt_valid;
98-
99-
// Constraint workspace
100-
Eigen::VectorXd y_combined;
101-
Eigen::VectorXd s_combined;
102-
Eigen::VectorXd g_combined;
103-
Eigen::MatrixXd Q_yu_combined;
104-
Eigen::MatrixXd Q_yx_combined;
105-
Eigen::MatrixXd YSinv;
106-
Eigen::MatrixXd bigRHS;
107-
108-
// Forward pass workspace
109-
std::vector<Eigen::VectorXd> delta_x_vectors;
110-
111-
bool initialized = false;
112-
} workspace_;
118+
double phi_ = 0.0; ///< Current filter merit value
119+
double theta_ = 0.0; ///< Current filter violation metric
113120

114121
// === Private helper methods ===
115-
void precomputeDynamicsDerivatives(CDDP &context);
116122
void precomputeConstraintGradients(CDDP &context);
117123
void evaluateTrajectory(CDDP &context);
118124
void evaluateTrajectoryWarmStart(CDDP &context);
@@ -126,6 +132,40 @@ namespace cddp
126132
double computeMaxConstraintViolation(const CDDP &context) const;
127133
double computeScaledDualInfeasibility(const CDDP &context) const;
128134

135+
// Filter and merit function methods
136+
bool acceptFilterEntry(double merit_function, double constraint_violation);
137+
bool isFilterAcceptable(double merit_function,
138+
double constraint_violation) const;
139+
140+
double computeTheta(
141+
const CDDPOptions &options,
142+
const std::map<std::string, std::vector<Eigen::VectorXd>> &constraints,
143+
const std::map<std::string, std::vector<Eigen::VectorXd>> &slacks,
144+
const std::map<std::string, Eigen::VectorXd> *terminal_constraints = nullptr,
145+
const std::map<std::string, Eigen::VectorXd> *terminal_slacks = nullptr,
146+
const Eigen::VectorXd *terminal_equality_residual = nullptr) const;
147+
148+
double computeBarrierMerit(
149+
const CDDP &context,
150+
const std::map<std::string, std::vector<Eigen::VectorXd>> &slacks,
151+
double cost,
152+
const std::map<std::string, Eigen::VectorXd> *terminal_slacks = nullptr,
153+
const Eigen::VectorXd *terminal_equality_multipliers = nullptr,
154+
const Eigen::VectorXd *terminal_equality_residual = nullptr) const;
155+
156+
std::pair<double, double> computePrimalAndComplementarity(
157+
const CDDP &context,
158+
const std::map<std::string, std::vector<Eigen::VectorXd>> &constraints,
159+
const std::map<std::string, std::vector<Eigen::VectorXd>> &slacks,
160+
const std::map<std::string, std::vector<Eigen::VectorXd>> &duals,
161+
double mu,
162+
const std::map<std::string, Eigen::VectorXd> *terminal_constraints = nullptr,
163+
const std::map<std::string, Eigen::VectorXd> *terminal_slacks = nullptr,
164+
const std::map<std::string, Eigen::VectorXd> *terminal_duals = nullptr,
165+
const Eigen::VectorXd *terminal_equality_residual = nullptr) const;
166+
167+
std::pair<double, double> computeMaxStepSizes(const CDDP &context) const;
168+
129169
// Legacy print helper (used by printIteration override)
130170
void printIterationLegacy(int iter, double objective, double inf_pr,
131171
double inf_du, double inf_comp, double mu,

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

Lines changed: 42 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -143,9 +143,49 @@ namespace cddp
143143
};
144144

145145
/**
146-
* @brief Options for the IPDDP solver.
146+
* @brief Comprehensive options for the IPDDP (Interior-Point DDP) solver.
147147
*/
148-
struct IPDDPAlgorithmOptions : InteriorPointOptions {};
148+
struct IPDDPAlgorithmOptions
149+
{
150+
double dual_var_init_scale = 1e-1; ///< Initial scale for dual variables.
151+
double slack_var_init_scale = 1e-2; ///< Initial scale for slack variables.
152+
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+
156+
double barrier_tol_mult =
157+
0.1; ///< Barrier-scaled inner tolerance multiplier.
158+
double barrier_update_dual_weight =
159+
0.01; ///< Down-weight inf_du for IPOPT-style barrier updates.
160+
double mu_kappa_epsilon =
161+
10.0; ///< IPOPT-style KKT trigger multiplier for non-adaptive updates.
162+
bool check_state_stationarity =
163+
false; ///< Include state-stationarity in inf_du when true.
164+
std::string theta_norm =
165+
"l1"; ///< Constraint violation norm used by the filter ("l1" or "l2").
166+
int max_filter_size =
167+
50; ///< Maximum number of active non-dominated filter entries.
168+
double theta_0_floor =
169+
1.0; ///< Minimum theta_0 value used for filter initialization.
170+
171+
bool warmstart_repair =
172+
false; ///< Repair warm-started slack/dual variables into the strict interior.
173+
double warmstart_s_min =
174+
1e-4; ///< Minimum slack value for warm-start repair.
175+
double warmstart_y_min =
176+
1e-4; ///< Minimum dual value for warm-start repair.
177+
double warmstart_interior_factor =
178+
1.1; ///< Multiplicative safety margin when warm-start values are too close to the boundary.
179+
double warmstart_reset_x0_threshold =
180+
-1.0; ///< Reset warm-start arrays when |x0 - X[0]| exceeds this threshold (<=0 disables).
181+
182+
double jacobian_regularization_value =
183+
1e-8; ///< Base reduced-system regularization floor for terminal equality LQR.
184+
double jacobian_regularization_exponent =
185+
0.25; ///< Exponent for mu-dependent reduced-system regularization.
186+
187+
SolverSpecificBarrierOptions barrier; ///< Barrier method parameters for IPDDP.
188+
};
149189

150190
/**
151191
* @brief Options for the MSIPDDP solver (interior-point + multi-shooting).

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

Lines changed: 23 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -33,23 +33,26 @@ namespace cddp
3333

3434
Eigen::MatrixXd getControlJacobian(
3535
const Eigen::VectorXd &/*state*/,
36-
const Eigen::VectorXd &control // control.size() gives control_dim
36+
const Eigen::VectorXd &control, // control.size() gives control_dim
37+
int /*index*/ = 0
3738
) const override
3839
{
3940
return Eigen::MatrixXd::Zero(getDualDim(), control.size());
4041
}
4142

4243
std::vector<Eigen::MatrixXd> getControlHessian(
4344
const Eigen::VectorXd &/*state*/,
44-
const Eigen::VectorXd &control
45+
const Eigen::VectorXd &control,
46+
int /*index*/ = 0
4547
) const override
4648
{
4749
return {};
4850
}
4951

5052
std::vector<Eigen::MatrixXd> getCrossHessian(
5153
const Eigen::VectorXd &state,
52-
const Eigen::VectorXd &control
54+
const Eigen::VectorXd &control,
55+
int /*index*/ = 0
5356
) const override
5457
{
5558
return {};
@@ -70,7 +73,8 @@ namespace cddp
7073
}
7174

7275
Eigen::VectorXd evaluate(const Eigen::VectorXd &final_state,
73-
const Eigen::VectorXd &/*control_is_ignored*/) const override
76+
const Eigen::VectorXd &/*control_is_ignored*/,
77+
int /*index*/ = 0) const override
7478
{
7579
if (final_state.size() != target_state_.size()) {
7680
throw std::invalid_argument("TerminalEqualityConstraint: final_state dimension mismatch.");
@@ -95,7 +99,8 @@ namespace cddp
9599
}
96100

97101
Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd &final_state,
98-
const Eigen::VectorXd &/*control_is_ignored*/) const override
102+
const Eigen::VectorXd &/*control_is_ignored*/,
103+
int /*index*/ = 0) const override
99104
{
100105
if (final_state.size() != target_state_.size()) {
101106
throw std::invalid_argument("TerminalEqualityConstraint: final_state dimension mismatch for Jacobian.");
@@ -109,7 +114,8 @@ namespace cddp
109114
}
110115

111116
double computeViolation(const Eigen::VectorXd &final_state,
112-
const Eigen::VectorXd &control) const override
117+
const Eigen::VectorXd &control,
118+
int /*index*/ = 0) const override
113119
{
114120
Eigen::VectorXd g = evaluate(final_state, control);
115121
return computeViolationFromValue(g);
@@ -130,7 +136,8 @@ namespace cddp
130136
// Hessians for g(x_N) = x_N - target are zero
131137
std::vector<Eigen::MatrixXd> getStateHessian(
132138
const Eigen::VectorXd &state,
133-
const Eigen::VectorXd &/*control_is_ignored*/
139+
const Eigen::VectorXd &/*control_is_ignored*/,
140+
int /*index*/ = 0
134141
) const override
135142
{
136143
std::vector<Eigen::MatrixXd> Hxx_list;
@@ -171,7 +178,8 @@ namespace cddp
171178
}
172179

173180
Eigen::VectorXd evaluate(const Eigen::VectorXd &final_state,
174-
const Eigen::VectorXd &/*control_is_ignored*/) const override
181+
const Eigen::VectorXd &/*control_is_ignored*/,
182+
int /*index*/ = 0) const override
175183
{
176184
if (final_state.size() != A_N_.cols()) {
177185
throw std::invalid_argument("TerminalInequalityConstraint: final_state dimension and A_N columns mismatch.");
@@ -196,7 +204,8 @@ namespace cddp
196204
}
197205

198206
Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd &final_state,
199-
const Eigen::VectorXd &/*control_is_ignored*/) const override
207+
const Eigen::VectorXd &/*control_is_ignored*/,
208+
int /*index*/ = 0) const override
200209
{
201210
if (final_state.size() != A_N_.cols()) {
202211
throw std::invalid_argument("TerminalStateInequalityConstraint: final_state dimension for Jacobian and A_N columns mismatch.");
@@ -210,7 +219,8 @@ namespace cddp
210219
}
211220

212221
double computeViolation(const Eigen::VectorXd &final_state,
213-
const Eigen::VectorXd &control) const override
222+
const Eigen::VectorXd &control,
223+
int /*index*/ = 0) const override
214224
{
215225
Eigen::VectorXd g = evaluate(final_state, control);
216226
return computeViolationFromValue(g);
@@ -231,7 +241,8 @@ namespace cddp
231241
// Hessians for g(x_N) = A_N * x_N - b_N are zero
232242
std::vector<Eigen::MatrixXd> getStateHessian(
233243
const Eigen::VectorXd &state,
234-
const Eigen::VectorXd &/*control_is_ignored*/
244+
const Eigen::VectorXd &/*control_is_ignored*/,
245+
int /*index*/ = 0
235246
) const override
236247
{
237248
std::vector<Eigen::MatrixXd> Hxx_list;
@@ -251,4 +262,4 @@ namespace cddp
251262
Eigen::VectorXd b_N_;
252263
};
253264
} // namespace cddp
254-
#endif // CDDP_TERMINAL_CONSTRAINT_HPP
265+
#endif // CDDP_TERMINAL_CONSTRAINT_HPP

0 commit comments

Comments
 (0)