Skip to content

Fix LogDDP single-shooting regressions #166

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
astomodynamics merged 2 commits into from
Apr 3, 2026
Merged

Fix LogDDP single-shooting regressions #166

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4106bb4
Fix LogDDP single-shooting regressions
astomodynamics Apr 3, 2026
68eaf60
Fix stale barrier params in cold-start re-initialization
Copilot Apr 3, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 0 additions & 6 deletions include/cddp-cpp/cddp_core/logddp_solver.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,9 +56,6 @@ class LogDDPSolver : public CDDPSolverBase {
void printIteration(int iter, const CDDP &context) const override;

private:
// Dynamics storage (forward-simulated trajectory)
std::vector<Eigen::VectorXd> F_;

// Constraint values g(x,u) - g_ub
std::map<std::string, std::vector<Eigen::VectorXd>> G_;

Expand All @@ -70,9 +67,6 @@ class LogDDPSolver : public CDDPSolverBase {
// Filter-based line search
double constraint_violation_;

// Multi-shooting parameters
int ms_segment_length_;

/**
* @brief Evaluate trajectory by computing cost, dynamics, and merit function.
*/
Expand Down
6 changes: 2 additions & 4 deletions include/cddp-cpp/cddp_core/options.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -130,12 +130,10 @@ namespace cddp
};

/**
* @brief Options for the relaxed log-barrier method.
* @brief Options for the relaxed log-barrier method (single-shooting).
*/
struct LogBarrierOptions : MultiShootingOptions
struct LogBarrierOptions
{
LogBarrierOptions() { use_controlled_rollout = true; }

bool use_relaxed_log_barrier_penalty =
false; ///< Use relaxed log-barrier method.
double relaxed_log_barrier_delta =
Expand Down
6 changes: 1 addition & 5 deletions python/src/bind_options.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,13 +74,9 @@ void bind_options(py::module_& m) {
.def_readwrite("slack_var_init_scale", &cddp::InteriorPointOptions::slack_var_init_scale)
.def_readwrite("barrier", &cddp::InteriorPointOptions::barrier);

// LogBarrierOptions exposed as a flat Python binding; no Python base class.
// LogBarrierOptions exposed as a flat Python binding; single-shooting only.
py::class_<cddp::LogBarrierOptions>(m, "LogBarrierOptions")
.def(py::init<>())
.def_readwrite("segment_length", &cddp::LogBarrierOptions::segment_length)
.def_readwrite("rollout_type", &cddp::LogBarrierOptions::rollout_type)
.def_readwrite("use_controlled_rollout", &cddp::LogBarrierOptions::use_controlled_rollout)
.def_readwrite("costate_var_init_scale", &cddp::LogBarrierOptions::costate_var_init_scale)
.def_readwrite("use_relaxed_log_barrier_penalty", &cddp::LogBarrierOptions::use_relaxed_log_barrier_penalty)
.def_readwrite("relaxed_log_barrier_delta", &cddp::LogBarrierOptions::relaxed_log_barrier_delta)
.def_readwrite("barrier", &cddp::LogBarrierOptions::barrier);
Expand Down
143 changes: 41 additions & 102 deletions src/cddp_core/logddp_solver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,9 +25,22 @@ limitations under the License.

namespace cddp {

namespace {

void rollOutNominalTrajectory(CDDP &context) {
const int horizon = context.getHorizon();

context.X_[0] = context.getInitialState();
for (int t = 0; t < horizon; ++t) {
context.X_[t + 1] = context.getSystem().getDiscreteDynamics(
context.X_[t], context.U_[t], t * context.getTimestep());
}
}

} // namespace

LogDDPSolver::LogDDPSolver()
: mu_(1e-1), relaxation_delta_(1e-5), constraint_violation_(1e+7),
ms_segment_length_(5) {}
: mu_(1e-1), relaxation_delta_(1e-5), constraint_violation_(1e+7) {}

void LogDDPSolver::initialize(CDDP &context) {
const CDDPOptions &options = context.getOptions();
Expand All @@ -52,14 +65,13 @@ void LogDDPSolver::initialize(CDDP &context) {
bool valid_warm_start =
(k_u_.size() == static_cast<size_t>(horizon) &&
K_u_.size() == static_cast<size_t>(horizon) &&
F_.size() == static_cast<size_t>(horizon) &&
context.X_.size() == static_cast<size_t>(horizon + 1) &&
context.U_.size() == static_cast<size_t>(horizon));

if (valid_warm_start && !k_u_.empty()) {
for (int t = 0; t < horizon; ++t) {
if (k_u_[t].size() != control_dim || K_u_[t].rows() != control_dim ||
K_u_[t].cols() != state_dim || F_[t].size() != state_dim) {
K_u_[t].cols() != state_dim) {
valid_warm_start = false;
break;
}
Expand Down Expand Up @@ -99,7 +111,11 @@ void LogDDPSolver::initialize(CDDP &context) {
dV_ = Eigen::Vector2d::Zero();

constraint_violation_ = std::numeric_limits<double>::infinity();
ms_segment_length_ = options.log_barrier.segment_length;

// Warm starts may reuse gains with a user-modified state trajectory.
// Re-roll the nominal state sequence so the linearization point stays
// dynamically consistent after defect tracking removal.
rollOutNominalTrajectory(context);

// Evaluate current trajectory and reset filter
evaluateTrajectory(context);
Expand All @@ -112,8 +128,9 @@ void LogDDPSolver::initialize(CDDP &context) {
}
}

// Cold start: Initialize trajectories with interpolation between initial and
// reference states
// Cold start: initialize a nominal trajectory and always roll it out from
// the current control guess so the linearization point is dynamically
// consistent, even when the user provided a same-sized state guess.
if (context.X_.size() != static_cast<size_t>(horizon + 1) ||
context.U_.size() != static_cast<size_t>(horizon)) {
context.X_.resize(horizon + 1);
Expand All @@ -133,11 +150,9 @@ void LogDDPSolver::initialize(CDDP &context) {
}
}

// Resize dynamics storage
F_.resize(horizon);
for (int t = 0; t < horizon; ++t) {
F_[t] = Eigen::VectorXd::Zero(state_dim);
}
// Roll out the nominal trajectory for all cold starts, including user-
// supplied trajectories whose state sequence is only an initial guess.
rollOutNominalTrajectory(context);

// Use base class helper for gains
initializeGains(horizon, control_dim, state_dim);
Expand Down Expand Up @@ -173,28 +188,15 @@ void LogDDPSolver::initialize(CDDP &context) {

constraint_violation_ = std::numeric_limits<double>::infinity();

ms_segment_length_ = options.log_barrier.segment_length;

// Check if ms_segment_length_ is valid
if (ms_segment_length_ < 0) {
std::cerr << "LogDDP: ms_segment_length_ must be non-negative" << std::endl;
throw std::runtime_error("LogDDP: ms_segment_length_ must be non-negative");
}

const std::string &rollout_type = options.log_barrier.rollout_type;
if (rollout_type != "linear" && rollout_type != "nonlinear" &&
rollout_type != "hybrid") {
std::cerr << "LogDDP: Invalid ms_rollout_type: " << rollout_type
<< std::endl;
throw std::runtime_error("LogDDP: Invalid ms_rollout_type");
}

// Initialize log barrier object
mu_ = options.log_barrier.barrier.mu_initial;
relaxation_delta_ = options.log_barrier.relaxed_log_barrier_delta;
Comment thread
astomodynamics marked this conversation as resolved.
if (!relaxed_log_barrier_) {
relaxed_log_barrier_ =
std::make_unique<RelaxedLogBarrier>(mu_, relaxation_delta_);
} else {
relaxed_log_barrier_->setBarrierCoeff(mu_);
relaxed_log_barrier_->setRelaxationDelta(relaxation_delta_);
}

// Evaluate initial trajectory
Expand Down Expand Up @@ -225,9 +227,6 @@ void LogDDPSolver::applyForwardPassResult(CDDP &context,
CDDPSolverBase::applyForwardPassResult(context, result);

// Also update LogDDP-specific state
if (result.dynamics_trajectory) {
F_ = *result.dynamics_trajectory;
}
constraint_violation_ = result.constraint_violation;
}

Expand Down Expand Up @@ -319,17 +318,9 @@ void LogDDPSolver::evaluateTrajectory(CDDP &context) {
const int horizon = context.getHorizon();
double cost = 0.0;

// Rollout dynamics and calculate cost
// Calculate cost over the (already consistent) trajectory
for (int t = 0; t < horizon; ++t) {
const Eigen::VectorXd &x_t = context.X_[t];
const Eigen::VectorXd &u_t = context.U_[t];

// Compute stage cost using the guessed state/control
cost += context.getObjective().running_cost(x_t, u_t, t);

// Compute dynamics
F_[t] = context.getSystem().getDiscreteDynamics(x_t, u_t,
t * context.getTimestep());
cost += context.getObjective().running_cost(context.X_[t], context.U_[t], t);
}

// Add terminal cost based on the final guessed state
Expand All @@ -343,7 +334,6 @@ void LogDDPSolver::resetFilter(CDDP &context) {
// Evaluate log-barrier cost (includes path constraints)
context.merit_function_ = context.cost_;
constraint_violation_ = 0.0;
double defect_violation = 0.0;

const auto &constraint_set = context.getConstraintSet();

Expand All @@ -364,12 +354,7 @@ void LogDDPSolver::resetFilter(CDDP &context) {
}
}
}

// Add defect violation penalty
Eigen::VectorXd d = F_[t] - context.X_[t + 1];
defect_violation += d.lpNorm<1>();
}
constraint_violation_ += defect_violation;
context.inf_pr_ = constraint_violation_;
}

Expand Down Expand Up @@ -502,8 +487,6 @@ bool LogDDPSolver::backwardPass(CDDP &context) {
for (int t = horizon - 1; t >= 0; --t) {
const Eigen::VectorXd &x = context.X_[t];
const Eigen::VectorXd &u = context.U_[t];
const Eigen::VectorXd &f = F_[t];
const Eigen::VectorXd &d = f - context.X_[t + 1]; // Defect

// Use pre-computed continuous-time dynamics Jacobians
const Eigen::MatrixXd &Fx = F_x_[t];
Expand All @@ -517,8 +500,8 @@ bool LogDDPSolver::backwardPass(CDDP &context) {
auto [l_xx, l_uu, l_ux] =
context.getObjective().getRunningCostHessians(x, u, t);

Eigen::VectorXd Q_x = l_x + A.transpose() * (V_x + V_xx * d);
Eigen::VectorXd Q_u = l_u + B.transpose() * (V_x + V_xx * d);
Eigen::VectorXd Q_x = l_x + A.transpose() * V_x;
Eigen::VectorXd Q_u = l_u + B.transpose() * V_x;
Eigen::MatrixXd Q_xx = l_xx + A.transpose() * V_xx * A;
Eigen::MatrixXd Q_ux = l_ux + B.transpose() * V_xx * A;
Eigen::MatrixXd Q_uu = l_uu + B.transpose() * V_xx * B;
Expand Down Expand Up @@ -631,61 +614,22 @@ ForwardPassResult LogDDPSolver::forwardPass(CDDP &context, double alpha) {
result.control_trajectory = context.U_;
result.state_trajectory[0] = context.getInitialState();

std::vector<Eigen::VectorXd> F_new = F_;

double cost_new = 0.0;
double merit_function_new = 0.0;
double rp_err = 0.0;
double rf_err = 0.0;

// Rollout loop with multi-shooting logic from original
// Single-shooting rollout
for (int t = 0; t < horizon; ++t) {
const Eigen::VectorXd delta_x_t =
result.state_trajectory[t] - context.X_[t];

// Update control
result.control_trajectory[t] =
context.U_[t] + alpha * k_u_[t] + K_u_[t] * delta_x_t;

// --- Rollout Logic from original ---
Eigen::VectorXd dynamics_eval_for_F_new_t;

// Determine if the *next* step (t+1) starts a new segment boundary
bool is_segment_boundary = (ms_segment_length_ > 0) &&
((t + 1) % ms_segment_length_ == 0) &&
(t + 1 < horizon);
bool apply_gap_closing_strategy = is_segment_boundary;

if (apply_gap_closing_strategy) {
if (options.log_barrier.rollout_type == "nonlinear") {
F_new[t] = context.getSystem().getDiscreteDynamics(
result.state_trajectory[t], result.control_trajectory[t],
t * context.getTimestep());
result.state_trajectory[t + 1] = context.X_[t + 1] +
(F_new[t] - F_[t]) +
alpha * (F_[t] - context.X_[t + 1]);
} else if (options.log_barrier.rollout_type == "hybrid") {
F_new[t] = context.getSystem().getDiscreteDynamics(
result.state_trajectory[t], result.control_trajectory[t],
t * context.getTimestep());
const auto [Fx, Fu] = context.getSystem().getJacobians(
context.X_[t], context.U_[t], t * context.getTimestep());
const Eigen::MatrixXd A =
Eigen::MatrixXd::Identity(state_dim, state_dim) +
context.getTimestep() * Fx;
const Eigen::MatrixXd B = context.getTimestep() * Fu;
result.state_trajectory[t + 1] =
context.X_[t + 1] + (A + B * K_u_[t]) * delta_x_t +
alpha * (B * k_u_[t] + F_[t] - context.X_[t + 1]);
}
} else {
result.state_trajectory[t + 1] = context.getSystem().getDiscreteDynamics(
result.state_trajectory[t], result.control_trajectory[t],
t * context.getTimestep());
F_new[t] = result.state_trajectory[t + 1];
}
result.state_trajectory[t + 1] = context.getSystem().getDiscreteDynamics(
result.state_trajectory[t], result.control_trajectory[t],
t * context.getTimestep());

// Robustness check
if (!result.state_trajectory[t + 1].allFinite() ||
!result.control_trajectory[t].allFinite()) {
if (options.debug) {
Expand All @@ -698,13 +642,12 @@ ForwardPassResult LogDDPSolver::forwardPass(CDDP &context, double alpha) {
}
}

// Cost computation and filter line-search from original
// Cost computation and constraint evaluation
for (int t = 0; t < horizon; ++t) {
cost_new += context.getObjective().running_cost(
result.state_trajectory[t], result.control_trajectory[t], t);

for (const auto &constraint_pair : constraint_set) {
const std::string &constraint_name = constraint_pair.first;
Eigen::VectorXd g_t =
constraint_pair.second->evaluate(result.state_trajectory[t],
result.control_trajectory[t]) -
Expand All @@ -719,18 +662,15 @@ ForwardPassResult LogDDPSolver::forwardPass(CDDP &context, double alpha) {
}
}
}

Eigen::VectorXd d = F_new[t] - result.state_trajectory[t + 1];
rf_err += d.lpNorm<1>();
}

cost_new +=
context.getObjective().terminal_cost(result.state_trajectory.back());
merit_function_new += cost_new;

// Filter-based acceptance using original logic with new options structure
// Filter-based acceptance
double constraint_violation_old = constraint_violation_;
double constraint_violation_new = rf_err + rp_err;
double constraint_violation_new = rp_err;
double merit_function_old = context.merit_function_;
bool filter_acceptance = false;
double expected_improvement = alpha * dV_(0);
Expand Down Expand Up @@ -766,7 +706,6 @@ ForwardPassResult LogDDPSolver::forwardPass(CDDP &context, double alpha) {
result.cost = cost_new;
result.merit_function = merit_function_new;
result.constraint_violation = constraint_violation_new;
result.dynamics_trajectory = F_new;
}

return result;
Expand Down
Loading
Loading