Skip to content

Commit 9cddf0f

Browse files
astomodynamicsastomodynamics
authored
Merge pull request #167 from astomodynamics/copilot/find-bugs-in-code
Fix CDDP reference/objective synchronization and constraint dual-dimension accounting
2 parents 7deffff + 55dbd3f commit 9cddf0f

2 files changed

Lines changed: 168 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: 150 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>();
@@ -214,6 +262,8 @@ std::unique_ptr<ISolverAlgorithm> createAnotherMockSolver() {
214262

215263
} // namespace cddp
216264

265+
using cddp::FixedDualDimConstraint;
266+
217267
// Test fixture for CDDP core functionality
218268
class CDDPCoreTest : public ::testing::Test {
219269
protected:
@@ -493,3 +543,103 @@ TEST_F(CDDPCoreTest, IntegrationWithTrajectoryAndOptions) {
493543
EXPECT_EQ(solution.state_trajectory.size(), horizon + 1);
494544
EXPECT_EQ(solution.control_trajectory.size(), horizon);
495545
}
546+
547+
TEST_F(CDDPCoreTest, SetReferenceStatesUpdatesObjectiveTerminalReference) {
548+
cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep,
549+
std::make_unique<cddp::Unicycle>(timestep, "euler"),
550+
std::make_unique<cddp::QuadraticObjective>(
551+
Eigen::MatrixXd::Identity(state_dim, state_dim),
552+
Eigen::MatrixXd::Identity(control_dim, control_dim),
553+
10.0 * Eigen::MatrixXd::Identity(state_dim, state_dim),
554+
goal_state, std::vector<Eigen::VectorXd>(), timestep),
555+
options);
556+
557+
std::vector<Eigen::VectorXd> reference_states(horizon + 1,
558+
Eigen::VectorXd::Zero(state_dim));
559+
for (int k = 0; k <= horizon; ++k) {
560+
reference_states[k] << 0.1 * k, 0.2 * k, 0.3 * k;
561+
}
562+
563+
cddp_solver.setReferenceStates(reference_states);
564+
565+
Eigen::VectorXd zero_control = Eigen::VectorXd::Zero(control_dim);
566+
EXPECT_TRUE(cddp_solver.getReferenceState().isApprox(reference_states.back()));
567+
EXPECT_NEAR(
568+
cddp_solver.getObjective().running_cost(reference_states.front(),
569+
zero_control, 0),
570+
0.0, 1e-12);
571+
EXPECT_NEAR(
572+
cddp_solver.getObjective().terminal_cost(reference_states.back()),
573+
0.0, 1e-12);
574+
}
575+
576+
TEST_F(CDDPCoreTest, SetObjectiveUsesExistingReferenceTrajectoryTerminalState) {
577+
cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep,
578+
std::make_unique<cddp::Unicycle>(timestep, "euler"),
579+
nullptr, options);
580+
581+
std::vector<Eigen::VectorXd> reference_states(horizon + 1,
582+
Eigen::VectorXd::Zero(state_dim));
583+
for (int k = 0; k < horizon; ++k) {
584+
reference_states[k] << 1.0 + 0.1 * k, 0.5 + 0.1 * k, 0.2 + 0.1 * k;
585+
}
586+
reference_states.back() = Eigen::VectorXd::Zero(state_dim);
587+
cddp_solver.setReferenceStates(reference_states);
588+
589+
cddp_solver.setObjective(std::make_unique<cddp::QuadraticObjective>(
590+
Eigen::MatrixXd::Identity(state_dim, state_dim),
591+
Eigen::MatrixXd::Identity(control_dim, control_dim),
592+
10.0 * Eigen::MatrixXd::Identity(state_dim, state_dim), goal_state,
593+
std::vector<Eigen::VectorXd>(), timestep));
594+
595+
Eigen::VectorXd zero_control = Eigen::VectorXd::Zero(control_dim);
596+
EXPECT_NEAR(
597+
cddp_solver.getObjective().running_cost(reference_states.front(),
598+
zero_control, 0),
599+
0.0, 1e-12);
600+
EXPECT_NEAR(
601+
cddp_solver.getObjective().terminal_cost(reference_states.back()),
602+
0.0, 1e-12);
603+
}
604+
605+
TEST_F(CDDPCoreTest, ReplacingConstraintsKeepsTotalDualDimensionAccurate) {
606+
cddp::CDDP cddp_solver(initial_state, goal_state, horizon, timestep,
607+
std::make_unique<cddp::Unicycle>(timestep, "euler"),
608+
std::make_unique<cddp::QuadraticObjective>(
609+
Eigen::MatrixXd::Identity(state_dim, state_dim),
610+
Eigen::MatrixXd::Identity(control_dim, control_dim),
611+
10.0 * Eigen::MatrixXd::Identity(state_dim, state_dim),
612+
goal_state, std::vector<Eigen::VectorXd>(), timestep),
613+
options);
614+
615+
Eigen::VectorXd control_bound_two_dim = Eigen::VectorXd::Ones(control_dim);
616+
cddp_solver.addPathConstraint(
617+
"RepeatedPathConstraint",
618+
std::make_unique<cddp::ControlConstraint>(-control_bound_two_dim,
619+
control_bound_two_dim));
620+
EXPECT_EQ(cddp_solver.getTotalDualDim(), 2 * control_dim);
621+
622+
Eigen::VectorXd control_bound_one_dim = Eigen::VectorXd::Ones(1);
623+
cddp_solver.addPathConstraint(
624+
"RepeatedPathConstraint",
625+
std::make_unique<cddp::ControlConstraint>(-control_bound_one_dim,
626+
control_bound_one_dim));
627+
EXPECT_EQ(cddp_solver.getTotalDualDim(), 2);
628+
629+
cddp_solver.addTerminalConstraint(
630+
"RepeatedTerminalConstraint",
631+
std::make_unique<FixedDualDimConstraint>(state_dim));
632+
EXPECT_EQ(cddp_solver.getTotalDualDim(), 2 + state_dim);
633+
634+
cddp_solver.addTerminalConstraint(
635+
"RepeatedTerminalConstraint",
636+
std::make_unique<FixedDualDimConstraint>(1));
637+
EXPECT_EQ(cddp_solver.getTotalDualDim(), 3);
638+
639+
EXPECT_TRUE(cddp_solver.removePathConstraint("RepeatedPathConstraint"));
640+
EXPECT_EQ(cddp_solver.getTotalDualDim(), 1);
641+
642+
EXPECT_TRUE(
643+
cddp_solver.removeTerminalConstraint("RepeatedTerminalConstraint"));
644+
EXPECT_EQ(cddp_solver.getTotalDualDim(), 0);
645+
}

0 commit comments

Comments
 (0)