Skip to content

Commit d351fd4

Browse files
Refactor constraints to IP-compatible box formulation (#157)
This PR refactors box-style constraints to a single interior-point-friendly formulation and aligns all call sites and tests with the new API.
1 parent 12ba9fc commit d351fd4

44 files changed

Lines changed: 269 additions & 442 deletions

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

examples/cddp_acrobot.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -88,7 +88,7 @@ int main() {
8888

8989
// Add control constraint
9090
cddp_solver.addPathConstraint("ControlConstraint",
91-
std::make_unique<cddp::ControlConstraint>(control_upper_bound));
91+
std::make_unique<cddp::ControlConstraint>(-control_upper_bound, control_upper_bound));
9292

9393
// Initial trajectory
9494
std::vector<Eigen::VectorXd> X_init(horizon + 1, Eigen::VectorXd::Zero(state_dim));

examples/cddp_bicycle.cpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -55,9 +55,9 @@ int main()
5555
control_lower_bound << -10.0, -M_PI / 5;
5656
Eigen::VectorXd control_upper_bound(control_dim);
5757
control_upper_bound << 10.0, M_PI / 5;
58-
cddp_solver.addPathConstraint("ControlBoxConstraint",
59-
std::make_unique<cddp::ControlBoxConstraint>(control_lower_bound, control_upper_bound));
60-
auto constraint = cddp_solver.getConstraint<cddp::ControlBoxConstraint>("ControlBoxConstraint");
58+
cddp_solver.addPathConstraint("ControlConstraint",
59+
std::make_unique<cddp::ControlConstraint>(control_lower_bound, control_upper_bound));
60+
auto constraint = cddp_solver.getConstraint<cddp::ControlConstraint>("ControlConstraint");
6161
std::vector<Eigen::VectorXd> X(horizon + 1, Eigen::VectorXd::Zero(state_dim));
6262
std::vector<Eigen::VectorXd> U(horizon, Eigen::VectorXd::Zero(control_dim));
6363
cddp_solver.setInitialTrajectory(X, U);

examples/cddp_car.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -188,8 +188,8 @@ int main()
188188
options
189189
);
190190

191-
cddp_solver.addPathConstraint("ControlBoxConstraint",
192-
std::make_unique<cddp::ControlBoxConstraint>(
191+
cddp_solver.addPathConstraint("ControlConstraint",
192+
std::make_unique<cddp::ControlConstraint>(
193193
control_lower_bound, control_upper_bound));
194194

195195
// Initialize with random controls

examples/cddp_car_ipddp.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -187,7 +187,7 @@ int main() {
187187
control_lower_bound << -0.5, -2.0; // [steering_angle, acceleration]
188188
Eigen::VectorXd control_upper_bound(control_dim);
189189
control_upper_bound << 0.5, 2.0;
190-
cddp_solver.addPathConstraint("ControlConstraint", std::make_unique<cddp::ControlConstraint>(control_upper_bound));
190+
cddp_solver.addPathConstraint("ControlConstraint", std::make_unique<cddp::ControlConstraint>(-control_upper_bound, control_upper_bound));
191191

192192
// Initialize the trajectory with zero controls
193193
std::vector<Eigen::VectorXd> X(horizon + 1, Eigen::VectorXd::Zero(state_dim));

examples/cddp_cartpole.cpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -92,11 +92,11 @@ int main() {
9292

9393
// FIXME: For MSIPDDP
9494
cddp_solver.addPathConstraint("ControlConstraint",
95-
std::make_unique<cddp::ControlConstraint>( control_upper_bound));
95+
std::make_unique<cddp::ControlConstraint>(-control_upper_bound, control_upper_bound));
9696

9797
// FIXME: For CLDDP
98-
// cddp_solver.addPathConstraint("ControlBoxConstraint",
99-
// std::make_unique<cddp::ControlBoxConstraint>( control_lower_bound, control_upper_bound));
98+
// cddp_solver.addPathConstraint("ControlConstraint",
99+
// std::make_unique<cddp::ControlConstraint>( control_lower_bound, control_upper_bound));
100100

101101
// Initial trajectory.
102102
std::vector<Eigen::VectorXd> X(horizon + 1, Eigen::VectorXd::Zero(state_dim));

examples/cddp_forklift_ipddp.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -218,13 +218,13 @@ int main() {
218218
control_upper_bound << 1.5, 0.4;
219219
cddp_solver.addPathConstraint("ControlConstraint",
220220
std::make_unique<cddp::ControlConstraint>(
221-
control_upper_bound, control_lower_bound));
221+
control_lower_bound, control_upper_bound));
222222

223223
// State constraints
224224
Eigen::VectorXd state_upper_bound(2);
225225
state_upper_bound << 2.0, max_steering_angle;
226226
cddp_solver.addPathConstraint("StateConstraint",
227-
std::make_unique<cddp::StateConstraint>(state_upper_bound));
227+
std::make_unique<cddp::StateConstraint>(-state_upper_bound, state_upper_bound));
228228

229229
// Initialize trajectory for back-in parking maneuver
230230
std::vector<Eigen::VectorXd> X(horizon + 1, Eigen::VectorXd::Zero(state_dim));

examples/cddp_hcw.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -113,8 +113,8 @@ int main() {
113113
umin << -1.0, -1.0, -1.0;
114114
umax << 1.0, 1.0, 1.0;
115115

116-
cddp_solver.addPathConstraint("ControlBoxConstraint",
117-
std::make_unique<cddp::ControlBoxConstraint>(umin, umax));
116+
cddp_solver.addPathConstraint("ControlConstraint",
117+
std::make_unique<cddp::ControlConstraint>(umin, umax));
118118

119119
// Initialize the trajectory (X,U) with something nontrivial
120120
std::vector<Eigen::VectorXd> X(horizon + 1, Eigen::VectorXd::Zero(STATE_DIM));

examples/cddp_lti_system.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -109,8 +109,8 @@ int main() {
109109
Eigen::VectorXd control_lower_bound = -0.6 * Eigen::VectorXd::Ones(control_dim);
110110
Eigen::VectorXd control_upper_bound = 0.6 * Eigen::VectorXd::Ones(control_dim);
111111

112-
cddp_solver.addPathConstraint("ControlBoxConstraint",
113-
std::make_unique<cddp::ControlBoxConstraint>(
112+
cddp_solver.addPathConstraint("ControlConstraint",
113+
std::make_unique<cddp::ControlConstraint>(
114114
control_lower_bound, control_upper_bound));
115115

116116
// Initialize trajectories

examples/cddp_manipulator.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -73,8 +73,8 @@ int main() {
7373
double max_torque = 50.0;
7474
Eigen::VectorXd control_lower_bound = -max_torque * Eigen::VectorXd::Ones(control_dim);
7575
Eigen::VectorXd control_upper_bound = max_torque * Eigen::VectorXd::Ones(control_dim);
76-
cddp_solver.addPathConstraint("ControlBoxConstraint",
77-
std::make_unique<cddp::ControlBoxConstraint>(control_lower_bound, control_upper_bound));
76+
cddp_solver.addPathConstraint("ControlConstraint",
77+
std::make_unique<cddp::ControlConstraint>(control_lower_bound, control_upper_bound));
7878

7979
// Initialize trajectories: here we use linear interpolation between initial and goal state
8080
std::vector<Eigen::VectorXd> X(horizon + 1, Eigen::VectorXd::Zero(state_dim));

examples/cddp_pendulum.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -94,8 +94,8 @@ int main() {
9494
options
9595
);
9696

97-
cddp_solver.addPathConstraint("ControlBoxConstraint",
98-
std::make_unique<cddp::ControlBoxConstraint>(control_lower_bound, control_upper_bound));
97+
cddp_solver.addPathConstraint("ControlConstraint",
98+
std::make_unique<cddp::ControlConstraint>(control_lower_bound, control_upper_bound));
9999

100100
// Set initial trajectory for the solver
101101
std::vector<Eigen::VectorXd> X(horizon + 1, Eigen::VectorXd::Zero(state_dim));

0 commit comments

Comments
 (0)