Skip to content

Commit 7f1d5e6

Browse files
astomodynamicsastomodynamics
authored
Merge early version of ipddp to master (#77)
* Rename feedback_gain to control_gain in CDDP solution structure * Add getDualDim method to constraint classes * Add dual and slack trajectory support in CDDP structures * Add support for IPDDP solver in CDDP::solve method * Add getTotalDualDim method and update addConstraint to track total dual dimensions * Fix variable assignment for feedback gain in solveLogCDDPBackwardPass method * Add header and footer option to CDDP options and update verbose checks * Feasible annd infeasible ipddp dev is good to go; need improvement * fix multi-threading bug
1 parent b7dbb16 commit 7f1d5e6

17 files changed

+2683
-155
lines changed

‎.gitignore

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,4 +3,5 @@ build/
33
plots/
44
results/frames
55
models/
6-
neural_models/
6+
neural_models/
7+
ipddp/

‎CMakeLists.txt

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -200,6 +200,8 @@ set(cddp_core_srcs
200200
src/cddp_core/clddp_core.cpp
201201
src/cddp_core/asddp_core.cpp
202202
src/cddp_core/logddp_core.cpp
203+
src/cddp_core/ipddp_core.cpp
204+
src/cddp_core/ipddp_feasible_core.cpp
203205
)
204206

205207
if (CDDP_CPP_TORCH)

‎examples/cddp_pendulum.cpp

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -94,6 +94,9 @@ int main() {
9494
// Initial trajectory
9595
std::vector<Eigen::VectorXd> X(horizon + 1, Eigen::VectorXd::Zero(state_dim));
9696
std::vector<Eigen::VectorXd> U(horizon, Eigen::VectorXd::Zero(control_dim));
97+
for (int i = 0; i < horizon + 1; ++i) {
98+
X[i] = initial_state;
99+
}
97100
cddp_solver.setInitialTrajectory(X, U);
98101

99102
// Solve

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

Lines changed: 59 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -49,9 +49,15 @@ struct CDDPOptions {
4949
double backtracking_factor = std::pow(10, (-3.0/10.0)); // Factor for line search backtracking
5050
double minimum_reduction_ratio = 1e-6; // Minimum reduction for line search
5151

52+
// interior-point method
53+
double mu_initial = 1e-2; // Initial barrier coefficient
54+
double mu_min = 1e-8; // Minimum barrier coefficient
55+
double mu_max = 1e1; // Maximum barrier coefficient
56+
double mu_reduction_ratio = 0.1; // Factor for barrier coefficient
57+
5258
// log-barrier method
5359
double barrier_coeff = 1e-2; // Coefficient for log-barrier method
54-
double barrier_factor = 0.90; // Factor for log-barrier method
60+
double barrier_factor = 0.10; // Factor for log-barrier method
5561
double barrier_tolerance = 1e-8; // Tolerance for log-barrier method
5662
double relaxation_coeff = 1.0; // Relaxation for log-barrier method
5763
int barrier_order = 2; // Order for log-barrier method
@@ -76,6 +82,7 @@ struct CDDPOptions {
7682
// Other options
7783
bool verbose = true; // Option for debug printing
7884
bool debug = false; // Option for debug mode
85+
bool header_and_footer = true; // Option for header and footer
7986
bool is_ilqr = true; // Option for iLQR
8087
bool use_parallel = true; // Option for parallel computation
8188
int num_threads = max_line_search_iterations; // Number of threads to use
@@ -96,9 +103,13 @@ struct CDDPSolution {
96103
std::vector<double> time_sequence;
97104
std::vector<Eigen::VectorXd> control_sequence;
98105
std::vector<Eigen::VectorXd> state_sequence;
106+
std::vector<Eigen::VectorXd> dual_sequence;
107+
std::vector<Eigen::VectorXd> slack_sequence;
99108
std::vector<double> cost_sequence;
100109
std::vector<double> lagrangian_sequence;
101-
std::vector<Eigen::MatrixXd> feedback_gain;
110+
std::vector<Eigen::MatrixXd> control_gain;
111+
std::vector<Eigen::MatrixXd> dual_gain;
112+
std::vector<Eigen::MatrixXd> slack_gain;
102113
int iterations;
103114
double alpha;
104115
bool converged;
@@ -108,6 +119,8 @@ struct CDDPSolution {
108119
struct ForwardPassResult {
109120
std::vector<Eigen::VectorXd> state_sequence;
110121
std::vector<Eigen::VectorXd> control_sequence;
122+
std::map<std::string, std::vector<Eigen::VectorXd>> dual_sequence;
123+
std::map<std::string, std::vector<Eigen::VectorXd>> slack_sequence;
111124
double cost;
112125
double lagrangian;
113126
double alpha = 1.0;
@@ -143,6 +156,9 @@ class CDDP {
143156
const Eigen::VectorXd& getReferenceState() const { return reference_state_; }
144157
int getHorizon() const { return horizon_; }
145158
double getTimestep() const { return timestep_; }
159+
int getStateDim() const { return system_->getStateDim(); }
160+
int getControlDim() const { return system_->getControlDim(); }
161+
int getTotalDualDim() const { return total_dual_dim_; }
146162
const CDDPOptions& getOptions() const { return options_; }
147163

148164
// Setters
@@ -212,7 +228,14 @@ class CDDP {
212228
* @param constraint constraint object
213229
*/
214230
void addConstraint(std::string constraint_name, std::unique_ptr<Constraint> constraint) {
231+
// Insert into the map
215232
constraint_set_[constraint_name] = std::move(constraint);
233+
234+
// Increment total_dual_dim_
235+
int dim = constraint_set_[constraint_name]->getDualDim();
236+
total_dual_dim_ += dim;
237+
238+
initialized_ = false; // Reset the initialization flag
216239
}
217240

218241
/**
@@ -255,31 +278,49 @@ class CDDP {
255278

256279
private:
257280
// Solver methods
281+
// CLCDDP methods
258282
CDDPSolution solveCLCDDP();
259283
ForwardPassResult solveCLCDDPForwardPass(double alpha);
260284
bool solveCLCDDPBackwardPass();
261285

286+
// LogCDDP methods
262287
CDDPSolution solveLogCDDP();
263288
ForwardPassResult solveLogCDDPForwardPass(double alpha);
264289
bool solveLogCDDPBackwardPass();
265290

291+
// ASCDDP methods
266292
CDDPSolution solveASCDDP();
267293
ForwardPassResult solveASCDDPForwardPass(double alpha);
268294
bool solveASCDDPBackwardPass();
295+
296+
// IPDDP methods
297+
void initializeIPDDP();
298+
CDDPSolution solveIPDDP();
299+
ForwardPassResult solveIPDDPForwardPass(double alpha);
300+
bool solveIPDDPBackwardPass();
301+
302+
// Feasible IPDDP methods
303+
void initializeFeasibleIPDDP();
304+
CDDPSolution solveFeasibleIPDDP();
305+
ForwardPassResult solveFeasibleIPDDPForwardPass(double alpha);
306+
bool solveFeasibleIPDDPBackwardPass();
269307

270308
// Helper methods
271309
double computeConstraintViolation(const std::vector<Eigen::VectorXd>& X, const std::vector<Eigen::VectorXd>& U) const;
272310

273311
bool checkConvergence(double J_new, double J_old, double dJ, double expected_dV, double gradient_norm);
274312

275-
void printSolverInfo();
313+
// Regularization methods
314+
void increaseRegularization();
315+
void decreaseRegularization();
316+
bool isRegularizationLimitReached() const;
276317

318+
// Print methods
319+
void printSolverInfo();
277320
void printOptions(const CDDPOptions& options);
278-
279321
void printIteration(int iter, double cost, double lagrangian,
280322
double grad_norm, double lambda_state,
281323
double lambda_control, double alpha, double mu, double constraint_violation);
282-
283324
void printSolution(const CDDPSolution& solution);
284325

285326
private:
@@ -297,9 +338,13 @@ class CDDP {
297338
double timestep_; // Time step for the problem
298339
CDDPOptions options_; // Options for the solver
299340

341+
int total_dual_dim_ = 0; // Number of total dual variables across constraints
342+
300343
// Intermediate trajectories
301-
std::vector<Eigen::VectorXd> X_; // State trajectory
302-
std::vector<Eigen::VectorXd> U_; // Control trajectory
344+
std::vector<Eigen::VectorXd> X_; // State trajectory
345+
std::vector<Eigen::VectorXd> U_; // Control trajectory
346+
std::map<std::string, std::vector<Eigen::VectorXd>> Y_; // Dual trajectory
347+
std::map<std::string, std::vector<Eigen::VectorXd>> S_; // Slack trajectory
303348

304349
// Cost and Lagrangian
305350
double J_; // Cost
@@ -316,10 +361,14 @@ class CDDP {
316361
double mu_; // Barrier coefficient
317362
double constraint_violation_; // Current constraint violation measure
318363
double gamma_; // Small value for filter acceptance
319-
364+
320365
// Feedforward and feedback gains
321-
std::vector<Eigen::VectorXd> k_;
322-
std::vector<Eigen::MatrixXd> K_;
366+
std::vector<Eigen::VectorXd> k_u_;
367+
std::vector<Eigen::MatrixXd> K_u_;
368+
std::map<std::string, std::vector<Eigen::VectorXd>> k_y_;
369+
std::map<std::string, std::vector<Eigen::MatrixXd>> K_y_;
370+
std::map<std::string, std::vector<Eigen::VectorXd>> k_s_;
371+
std::map<std::string, std::vector<Eigen::MatrixXd>> K_s_;
323372

324373
// Q-function matrices
325374
std::vector<Eigen::MatrixXd> Q_UU_;

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

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,8 @@ class Constraint {
3333
// Get the name of the constraint
3434
const std::string& getName() const { return name_; }
3535

36+
virtual int getDualDim() const { return 0; }
37+
3638
// Evaluate the constraint function: g(x, u)
3739
virtual Eigen::VectorXd evaluate(const Eigen::VectorXd& state,
3840
const Eigen::VectorXd& control) const = 0;
@@ -86,6 +88,10 @@ class ControlBoxConstraint : public Constraint {
8688
lower_bound_(lower_bound),
8789
upper_bound_(upper_bound) {}
8890

91+
int getDualDim() const override {
92+
return lower_bound_.size() + upper_bound_.size();
93+
}
94+
8995
Eigen::VectorXd evaluate(const Eigen::VectorXd& state,
9096
const Eigen::VectorXd& control) const override
9197
{
@@ -141,6 +147,10 @@ class StateBoxConstraint : public Constraint {
141147
: Constraint("StateBoxConstraint"),
142148
lower_bound_(lower_bound),
143149
upper_bound_(upper_bound) {}
150+
151+
int getDualDim() const override {
152+
return lower_bound_.size() + upper_bound_.size();
153+
}
144154

145155
Eigen::VectorXd evaluate(const Eigen::VectorXd& state,
146156
const Eigen::VectorXd& control) const override
@@ -199,6 +209,10 @@ class LinearConstraint : public Constraint {
199209
A_(A),
200210
b_(b),
201211
scale_factor_(scale_factor) {}
212+
213+
int getDualDim() const override {
214+
return b_.size();
215+
}
202216

203217
Eigen::VectorXd evaluate(const Eigen::VectorXd& state,
204218
const Eigen::VectorXd& control) const override
@@ -256,6 +270,10 @@ class BallConstraint : public Constraint {
256270
{
257271
}
258272

273+
int getDualDim() const override {
274+
return 1;
275+
}
276+
259277

260278
Eigen::VectorXd evaluate(const Eigen::VectorXd& state,
261279
const Eigen::VectorXd& control) const override

‎results/tests/dubincs_car_ipddp_test.png

27.9 KB
Loading

‎results/tests/pendulum_cddp_test.png

700 Bytes
Loading

‎src/cddp_core/cddp_core.cpp

Lines changed: 121 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -44,20 +44,50 @@ CDDP::CDDP(const Eigen::VectorXd &initial_state,
4444
initialized_(false)
4545
{
4646
initializeCDDP();
47-
if (options_.verbose) {
47+
if (options_.header_and_footer) {
4848
printSolverInfo();
4949
printOptions(options_);
5050
}
5151
}
5252

5353
cddp::CDDPSolution CDDP::solve(std::string solver_type) {
5454
if (solver_type == "CLCDDP" || solver_type == "CLDDP") {
55+
if (options_.verbose) {
56+
std::cout << "--------------------" << std::endl;
57+
std::cout << "Solving with CLCDDP" << std::endl;
58+
std::cout << "--------------------" << std::endl;
59+
}
5560
return solveCLCDDP();
5661
} else if (solver_type == "LogCDDP" || solver_type == "LogDDP") {
62+
if (options_.verbose) {
63+
std::cout << "--------------------" << std::endl;
64+
std::cout << "Solving with LogCDDP" << std::endl;
65+
std::cout << "--------------------" << std::endl;
66+
}
5767
return solveLogCDDP();
5868
} else if (solver_type == "ASCDDP" || solver_type == "ASDDP") {
69+
if (options_.verbose) {
70+
std::cout << "--------------------" << std::endl;
71+
std::cout << "Solving with ASCDDP" << std::endl;
72+
std::cout << "--------------------" << std::endl;
73+
}
5974
return solveASCDDP();
60-
} else {
75+
} else if (solver_type == "IPDDP") {
76+
if (options_.verbose) {
77+
std::cout << "--------------------" << std::endl;
78+
std::cout << "Solving with IPDDP" << std::endl;
79+
std::cout << "--------------------" << std::endl;
80+
}
81+
return solveIPDDP();
82+
} else if (solver_type == "FeasibleIPDDP") {
83+
if (options_.verbose) {
84+
std::cout << "--------------------" << std::endl;
85+
std::cout << "Solving with FeasibleIPDDP" << std::endl;
86+
std::cout << "--------------------" << std::endl;
87+
}
88+
return solveFeasibleIPDDP();
89+
} else
90+
{
6191
std::cerr << "CDDP::solve: Unknown solver type" << std::endl;
6292
throw std::runtime_error("CDDP::solve: Unknown solver type");
6393
}
@@ -174,8 +204,8 @@ void CDDP::initializeCDDP()
174204

175205

176206
// Initialize gains and value reduction
177-
k_.resize(horizon_, Eigen::VectorXd::Zero(control_dim));
178-
K_.resize(horizon_, Eigen::MatrixXd::Zero(control_dim, state_dim));
207+
k_u_.resize(horizon_, Eigen::VectorXd::Zero(control_dim));
208+
K_u_.resize(horizon_, Eigen::MatrixXd::Zero(control_dim, state_dim));
179209
dV_.resize(2);
180210

181211
// Initialize Q-function matrices: USED ONLY FOR ASCDDP
@@ -221,6 +251,93 @@ double CDDP::computeConstraintViolation(const std::vector<Eigen::VectorXd>& X,
221251
return total_violation;
222252
}
223253

254+
255+
void CDDP::increaseRegularization()
256+
{
257+
// For “state” or “both”
258+
if (options_.regularization_type == "state" ||
259+
options_.regularization_type == "both")
260+
{
261+
// Increase step
262+
regularization_state_step_ = std::max(
263+
regularization_state_step_ * options_.regularization_state_factor,
264+
options_.regularization_state_factor);
265+
266+
// Increase actual regularization
267+
regularization_state_ = std::max(
268+
regularization_state_ * regularization_state_step_,
269+
options_.regularization_state_min);
270+
}
271+
272+
// For “control” or “both”
273+
if (options_.regularization_type == "control" ||
274+
options_.regularization_type == "both")
275+
{
276+
// Increase step
277+
regularization_control_step_ = std::max(
278+
regularization_control_step_ * options_.regularization_control_factor,
279+
options_.regularization_control_factor);
280+
281+
// Increase actual regularization
282+
regularization_control_ = std::max(
283+
regularization_control_ * regularization_control_step_,
284+
options_.regularization_control_min);
285+
}
286+
}
287+
288+
289+
void CDDP::decreaseRegularization()
290+
{
291+
// For “state” or “both”
292+
if (options_.regularization_type == "state" ||
293+
options_.regularization_type == "both")
294+
{
295+
// Decrease step
296+
regularization_state_step_ = std::min(
297+
regularization_state_step_ / options_.regularization_state_factor,
298+
1.0 / options_.regularization_state_factor);
299+
300+
// Decrease actual regularization
301+
regularization_state_ *= regularization_state_step_;
302+
if (regularization_state_ < options_.regularization_state_min) {
303+
regularization_state_ = options_.regularization_state_min;
304+
}
305+
}
306+
307+
// For “control” or “both”
308+
if (options_.regularization_type == "control" ||
309+
options_.regularization_type == "both")
310+
{
311+
// Decrease step
312+
regularization_control_step_ = std::min(
313+
regularization_control_step_ / options_.regularization_control_factor,
314+
1.0 / options_.regularization_control_factor);
315+
316+
// Decrease actual regularization
317+
regularization_control_ *= regularization_control_step_;
318+
if (regularization_control_ < options_.regularization_control_min) {
319+
regularization_control_ = options_.regularization_control_min;
320+
}
321+
}
322+
}
323+
324+
325+
bool CDDP::isRegularizationLimitReached() const
326+
{
327+
bool state_limit = (regularization_state_ >= options_.regularization_state_max);
328+
bool control_limit = (regularization_control_ >= options_.regularization_control_max);
329+
330+
if (options_.regularization_type == "state")
331+
return state_limit;
332+
else if (options_.regularization_type == "control")
333+
return control_limit;
334+
else if (options_.regularization_type == "both")
335+
return (state_limit || control_limit);
336+
337+
// For “none” or unknown, no limit in practice
338+
return false;
339+
}
340+
224341
void CDDP::printSolverInfo()
225342
{
226343
std::cout << "\n";

0 commit comments

Comments
 (0)