Skip to content

Commit d52db59

Browse files
Copilotastomodynamics
Copilot
and
astomodynamics
authored
Fix reference and constraint bookkeeping
Agent-Logs-Url: https://github.com/astomodynamics/cddp-cpp/sessions/45b3682d-eebc-4e0b-a321-e992dff31213 Co-authored-by: astomodynamics <49183997+astomodynamics@users.noreply.github.com>
1 parent 7deffff commit d52db59

2 files changed

Lines changed: 166 additions & 9 deletions

File tree

‎src/cddp_core/cddp_core.cpp‎

Lines changed: 18 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -103,14 +103,15 @@ void CDDP::setReferenceState(const Eigen::VectorXd &reference_state) {
103103
void CDDP::setReferenceStates(
104104
const std::vector<Eigen::VectorXd> &reference_states) {
105105
reference_states_ = reference_states;
106+
if (!reference_states_.empty()) {
107+
reference_state_ = reference_states_.back();
108+
}
106109
if (objective_) {
110+
if (!reference_states_.empty()) {
111+
objective_->setReferenceState(reference_state_);
112+
}
107113
objective_->setReferenceStates(reference_states_);
108114
}
109-
if (!reference_states_.empty()) {
110-
reference_state_ =
111-
reference_states_
112-
.back(); // Update single reference state to the final one
113-
}
114115
}
115116

116117
void CDDP::setHorizon(int horizon) {
@@ -142,12 +143,12 @@ void CDDP::setOptions(const CDDPOptions &options) {
142143

143144
void CDDP::setObjective(std::unique_ptr<Objective> objective) {
144145
objective_ = std::move(objective);
145-
if (objective_ && !reference_state_.isZero() &&
146-
reference_state_.size() > 0) { // Check if reference_state is valid
147-
objective_->setReferenceState(reference_state_);
148-
}
149146
if (objective_ && !reference_states_.empty()) {
147+
objective_->setReferenceState(reference_state_);
150148
objective_->setReferenceStates(reference_states_);
149+
} else if (objective_ && !reference_state_.isZero() &&
150+
reference_state_.size() > 0) { // Check if reference_state is valid
151+
objective_->setReferenceState(reference_state_);
151152
}
152153
}
153154

@@ -189,6 +190,10 @@ void CDDP::addPathConstraint(std::string constraint_name,
189190

190191
// Get dual dimension BEFORE moving the constraint
191192
int dual_dim = constraint->getDualDim();
193+
auto existing_constraint = path_constraint_set_.find(constraint_name);
194+
if (existing_constraint != path_constraint_set_.end()) {
195+
total_dual_dim_ -= existing_constraint->second->getDualDim();
196+
}
192197

193198
path_constraint_set_[constraint_name] = std::move(constraint);
194199

@@ -224,6 +229,10 @@ void CDDP::addTerminalConstraint(std::string constraint_name,
224229

225230
// Get dual dimension BEFORE moving the constraint
226231
int dual_dim = constraint->getDualDim();
232+
auto existing_constraint = terminal_constraint_set_.find(constraint_name);
233+
if (existing_constraint != terminal_constraint_set_.end()) {
234+
total_dual_dim_ -= existing_constraint->second->getDualDim();
235+
}
227236

228237
terminal_constraint_set_[constraint_name] = std::move(constraint);
229238

‎tests/cddp_core/test_cddp_core.cpp‎

Lines changed: 148 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
*/
1616

1717
#include <iostream>
18+
#include <limits>
1819
#include <vector>
1920
#include <string>
2021
#include <memory>
@@ -203,6 +204,53 @@ class ThrowingPrecomputeSolver : public CDDPSolverBase {
203204
void printIteration(int iter, const CDDP &context) const override {}
204205
};
205206

207+
class FixedDualDimConstraint : public Constraint {
208+
public:
209+
explicit FixedDualDimConstraint(int dual_dim, const std::string& name = "FixedDualDimConstraint")
210+
: Constraint(name), dual_dim_(dual_dim) {}
211+
212+
int getDualDim() const override { return dual_dim_; }
213+
214+
Eigen::VectorXd evaluate(const Eigen::VectorXd& state,
215+
const Eigen::VectorXd& control,
216+
int index = 0) const override {
217+
return Eigen::VectorXd::Zero(dual_dim_);
218+
}
219+
220+
Eigen::VectorXd getLowerBound() const override {
221+
return Eigen::VectorXd::Constant(dual_dim_, -std::numeric_limits<double>::infinity());
222+
}
223+
224+
Eigen::VectorXd getUpperBound() const override {
225+
return Eigen::VectorXd::Zero(dual_dim_);
226+
}
227+
228+
Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state,
229+
const Eigen::VectorXd& control,
230+
int index = 0) const override {
231+
return Eigen::MatrixXd::Zero(dual_dim_, state.size());
232+
}
233+
234+
Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state,
235+
const Eigen::VectorXd& control,
236+
int index = 0) const override {
237+
return Eigen::MatrixXd::Zero(dual_dim_, control.size());
238+
}
239+
240+
double computeViolation(const Eigen::VectorXd& state,
241+
const Eigen::VectorXd& control,
242+
int index = 0) const override {
243+
return 0.0;
244+
}
245+
246+
double computeViolationFromValue(const Eigen::VectorXd& g) const override {
247+
return 0.0;
248+
}
249+
250+
private:
251+
int dual_dim_;
252+
};
253+
206254
// Factory functions for the mock solvers
207255
std::unique_ptr<ISolverAlgorithm> createMockExternalSolver() {
208256
return std::make_unique<MockExternalSolver>();
@@ -493,3 +541,103 @@ TEST_F(CDDPCoreTest, IntegrationWithTrajectoryAndOptions) {
493541
EXPECT_EQ(solution.state_trajectory.size(), horizon + 1);
494542
EXPECT_EQ(solution.control_trajectory.size(), horizon);
495543
}
544+
545+
TEST_F(CDDPCoreTest, SetReferenceStatesUpdatesObjectiveTerminalReference) {
546+
cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep,
547+
std::make_unique<cddp::Unicycle>(timestep, "euler"),
548+
std::make_unique<cddp::QuadraticObjective>(
549+
Eigen::MatrixXd::Identity(state_dim, state_dim),
550+
Eigen::MatrixXd::Identity(control_dim, control_dim),
551+
10.0 * Eigen::MatrixXd::Identity(state_dim, state_dim),
552+
goal_state, std::vector<Eigen::VectorXd>(), timestep),
553+
options);
554+
555+
std::vector<Eigen::VectorXd> reference_states(horizon + 1,
556+
Eigen::VectorXd::Zero(state_dim));
557+
for (int k = 0; k <= horizon; ++k) {
558+
reference_states[k] << 0.1 * k, 0.2 * k, 0.3 * k;
559+
}
560+
561+
cddp_solver.setReferenceStates(reference_states);
562+
563+
Eigen::VectorXd zero_control = Eigen::VectorXd::Zero(control_dim);
564+
EXPECT_TRUE(cddp_solver.getReferenceState().isApprox(reference_states.back()));
565+
EXPECT_NEAR(
566+
cddp_solver.getObjective().running_cost(reference_states.front(),
567+
zero_control, 0),
568+
0.0, 1e-12);
569+
EXPECT_NEAR(
570+
cddp_solver.getObjective().terminal_cost(reference_states.back()),
571+
0.0, 1e-12);
572+
}
573+
574+
TEST_F(CDDPCoreTest, SetObjectiveUsesExistingReferenceTrajectoryTerminalState) {
575+
cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep,
576+
std::make_unique<cddp::Unicycle>(timestep, "euler"),
577+
nullptr, options);
578+
579+
std::vector<Eigen::VectorXd> reference_states(horizon + 1,
580+
Eigen::VectorXd::Zero(state_dim));
581+
for (int k = 0; k < horizon; ++k) {
582+
reference_states[k] << 1.0 + 0.1 * k, 0.5 + 0.1 * k, 0.2 + 0.1 * k;
583+
}
584+
reference_states.back() = Eigen::VectorXd::Zero(state_dim);
585+
cddp_solver.setReferenceStates(reference_states);
586+
587+
cddp_solver.setObjective(std::make_unique<cddp::QuadraticObjective>(
588+
Eigen::MatrixXd::Identity(state_dim, state_dim),
589+
Eigen::MatrixXd::Identity(control_dim, control_dim),
590+
10.0 * Eigen::MatrixXd::Identity(state_dim, state_dim), goal_state,
591+
std::vector<Eigen::VectorXd>(), timestep));
592+
593+
Eigen::VectorXd zero_control = Eigen::VectorXd::Zero(control_dim);
594+
EXPECT_NEAR(
595+
cddp_solver.getObjective().running_cost(reference_states.front(),
596+
zero_control, 0),
597+
0.0, 1e-12);
598+
EXPECT_NEAR(
599+
cddp_solver.getObjective().terminal_cost(reference_states.back()),
600+
0.0, 1e-12);
601+
}
602+
603+
TEST_F(CDDPCoreTest, ReplacingConstraintsKeepsTotalDualDimensionAccurate) {
604+
cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep,
605+
std::make_unique<cddp::Unicycle>(timestep, "euler"),
606+
std::make_unique<cddp::QuadraticObjective>(
607+
Eigen::MatrixXd::Identity(state_dim, state_dim),
608+
Eigen::MatrixXd::Identity(control_dim, control_dim),
609+
10.0 * Eigen::MatrixXd::Identity(state_dim, state_dim),
610+
goal_state, std::vector<Eigen::VectorXd>(), timestep),
611+
options);
612+
613+
Eigen::VectorXd control_bound_two_dim = Eigen::VectorXd::Ones(control_dim);
614+
cddp_solver.addPathConstraint(
615+
"RepeatedPathConstraint",
616+
std::make_unique<cddp::ControlConstraint>(-control_bound_two_dim,
617+
control_bound_two_dim));
618+
EXPECT_EQ(cddp_solver.getTotalDualDim(), 2 * control_dim);
619+
620+
Eigen::VectorXd control_bound_one_dim = Eigen::VectorXd::Ones(1);
621+
cddp_solver.addPathConstraint(
622+
"RepeatedPathConstraint",
623+
std::make_unique<cddp::ControlConstraint>(-control_bound_one_dim,
624+
control_bound_one_dim));
625+
EXPECT_EQ(cddp_solver.getTotalDualDim(), 2);
626+
627+
cddp_solver.addTerminalConstraint(
628+
"RepeatedTerminalConstraint",
629+
std::make_unique<cddp::FixedDualDimConstraint>(state_dim));
630+
EXPECT_EQ(cddp_solver.getTotalDualDim(), 2 + state_dim);
631+
632+
cddp_solver.addTerminalConstraint(
633+
"RepeatedTerminalConstraint",
634+
std::make_unique<cddp::FixedDualDimConstraint>(1));
635+
EXPECT_EQ(cddp_solver.getTotalDualDim(), 3);
636+
637+
EXPECT_TRUE(cddp_solver.removePathConstraint("RepeatedPathConstraint"));
638+
EXPECT_EQ(cddp_solver.getTotalDualDim(), 1);
639+
640+
EXPECT_TRUE(
641+
cddp_solver.removeTerminalConstraint("RepeatedTerminalConstraint"));
642+
EXPECT_EQ(cddp_solver.getTotalDualDim(), 0);
643+
}

0 commit comments

Comments
 (0)