Skip to content

Simplify comments in the repo #173

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 3 commits into from
Apr 5, 2026
Merged

Simplify comments in the repo #173

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
659d908
Remove outdated defect-dynamics comments
Copilot Apr 5, 2026
a562702
Remove extra LogDDP comments
Copilot Apr 5, 2026
6739eb0
Remove redundant dynamics-model comments
Copilot Apr 5, 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
7 changes: 1 addition & 6 deletions src/cddp_core/logddp_solver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,7 @@ void LogDDPSolver::initialize(CDDP &context) {

// 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.
// dynamically consistent.
rollOutNominalTrajectory(context);

// Evaluate current trajectory and reset filter
Expand Down Expand Up @@ -368,11 +368,6 @@ bool LogDDPSolver::backwardPass(CDDP &context) {
const double timestep = context.getTimestep();
const auto &constraint_set = context.getConstraintSet();

// Pre-compute dynamics jacobians and hessians for all time steps
// Note: LogDDP needs continuous-time Jacobians for the backward pass
// because it handles defect terms (d = F[t] - X[t+1]) which require
// the A = dt*Fx + I, B = dt*Fu formulation. We compute these inline
// since the base precomputeDynamicsDerivatives stores discrete-time versions.
const int MIN_HORIZON_FOR_PARALLEL = 20;
const bool use_parallel =
options.enable_parallel && horizon >= MIN_HORIZON_FOR_PARALLEL;
Expand Down
32 changes: 1 addition & 31 deletions src/dynamics_model/bicycle.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,15 +32,12 @@ Eigen::VectorXd Bicycle::getContinuousDynamics(const Eigen::VectorXd &state,

Eigen::VectorXd state_dot = Eigen::VectorXd::Zero(STATE_DIM);

// Extract state variables
const double theta = state(STATE_THETA); // heading angle
const double v = state(STATE_V); // velocity

// Extract control variables
const double a = control(CONTROL_ACC); // acceleration
const double delta = control(CONTROL_DELTA); // steering angle

// Kinematic bicycle model equations
state_dot(STATE_X) = v * std::cos(theta); // dx/dt
state_dot(STATE_Y) = v * std::sin(theta); // dy/dt
state_dot(STATE_THETA) = (v / wheelbase_) * std::tan(delta); // dtheta/dt
Expand All @@ -56,16 +53,12 @@ Bicycle::getContinuousDynamicsAutodiff(const VectorXdual2nd &state,


VectorXdual2nd state_dot = VectorXdual2nd::Zero(STATE_DIM);

// Extract state variables (now dual2nd types)
const autodiff::dual2nd theta = state(STATE_THETA); // heading angle
const autodiff::dual2nd v = state(STATE_V); // velocity

// Extract control variables (now dual2nd types)
const autodiff::dual2nd a = control(CONTROL_ACC); // acceleration
const autodiff::dual2nd delta = control(CONTROL_DELTA); // steering angle

// Use ADL for math functions
state_dot(STATE_X) = v * cos(theta);
state_dot(STATE_Y) = v * sin(theta);
state_dot(STATE_THETA) = (v / wheelbase_) * tan(delta);
Expand All @@ -80,25 +73,17 @@ Eigen::MatrixXd Bicycle::getStateJacobian(const Eigen::VectorXd &state,

Eigen::MatrixXd A = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM);

// Extract state variables
const double theta = state(STATE_THETA); // heading angle
const double v = state(STATE_V); // velocity

// Extract control variables
const double delta = control(CONTROL_DELTA); // steering angle

// Compute partial derivatives with respect to state variables
// df1/dtheta = d(dx/dt)/dtheta
A(STATE_X, STATE_THETA) = -v * std::sin(theta);
// df1/dv = d(dx/dt)/dv
A(STATE_X, STATE_V) = std::cos(theta);

// df2/dtheta = d(dy/dt)/dtheta
A(STATE_Y, STATE_THETA) = v * std::cos(theta);
// df2/dv = d(dy/dt)/dv
A(STATE_Y, STATE_V) = std::sin(theta);

// df3/dv = d(dtheta/dt)/dv
A(STATE_THETA, STATE_V) = std::tan(delta) / wheelbase_;

return A;
Expand All @@ -110,17 +95,12 @@ Eigen::MatrixXd Bicycle::getControlJacobian(const Eigen::VectorXd &state,

Eigen::MatrixXd B = Eigen::MatrixXd::Zero(STATE_DIM, CONTROL_DIM);

// Extract state variables
const double v = state(STATE_V); // velocity

// Extract control variables
const double delta = control(CONTROL_DELTA); // steering angle

// Compute partial derivatives with respect to control variables
// df4/da = d(dv/dt)/da
B(STATE_V, CONTROL_ACC) = 1.0;

// df3/ddelta = d(dtheta/dt)/ddelta
B(STATE_THETA, CONTROL_DELTA) =
v / (wheelbase_ * std::pow(std::cos(delta), 2));

Expand All @@ -133,22 +113,16 @@ Bicycle::getStateHessian(const Eigen::VectorXd &state,

auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM);

// Extract state variables
const double theta = state(STATE_THETA); // heading angle
const double v = state(STATE_V); // velocity

// Second derivatives with respect to states
// d²(dx/dt)/dtheta²
hessians[STATE_X](STATE_THETA, STATE_THETA) = -v * std::cos(theta);

// d²(dx/dt)/(dtheta*dv)
hessians[STATE_X](STATE_THETA, STATE_V) = -std::sin(theta);
hessians[STATE_X](STATE_V, STATE_THETA) = -std::sin(theta);

// d²(dy/dt)/dtheta²
hessians[STATE_Y](STATE_THETA, STATE_THETA) = -v * std::sin(theta);

// d²(dy/dt)/(dtheta*dv)
hessians[STATE_Y](STATE_THETA, STATE_V) = std::cos(theta);
hessians[STATE_Y](STATE_V, STATE_THETA) = std::cos(theta);

Expand All @@ -161,18 +135,14 @@ Bicycle::getControlHessian(const Eigen::VectorXd &state,

auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM);

// Extract state variables
const double v = state(STATE_V); // velocity

// Extract control variables
const double delta = control(CONTROL_DELTA); // steering angle

// Second derivatives with respect to controls
// d²(dtheta/dt)/ddelta²
hessians[STATE_THETA](CONTROL_DELTA, CONTROL_DELTA) =
2.0 * v * std::sin(delta) / (wheelbase_ * std::pow(std::cos(delta), 3));

return hessians;
}

} // namespace cddp
} // namespace cddp
48 changes: 1 addition & 47 deletions src/dynamics_model/car.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,22 +13,17 @@ Car::Car(double timestep, double wheelbase, std::string integration_type)

Eigen::VectorXd Car::getDiscreteDynamics(
const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const {

// Extract states
const double x = state(STATE_X); // x position
const double y = state(STATE_Y); // y position
const double theta = state(STATE_THETA); // car angle
const double v = state(STATE_V); // velocity

// Extract controls
const double delta = control(CONTROL_DELTA); // steering angle
const double a = control(CONTROL_A); // acceleration

// Constants
const double d = wheelbase_; // distance between back and front axles
const double h = timestep_; // timestep

// Compute unit vector in car direction
const double cos_theta = std::cos(theta);
const double sin_theta = std::sin(theta);
Eigen::Vector2d z(cos_theta, sin_theta);
Expand All @@ -45,40 +40,30 @@ Eigen::VectorXd Car::getDiscreteDynamics(
// dtheta = asin(sin(w)*f/d)
const double dtheta = std::asin(std::sin(delta) * f / d);

// Compute state change
Eigen::VectorXd dy = Eigen::VectorXd::Zero(STATE_DIM);
dy(STATE_X) = b * cos_theta;
dy(STATE_Y) = b * sin_theta;
dy(STATE_THETA) = dtheta;
dy(STATE_V) = h * a;

// Return next state
return state + dy;
}

Eigen::MatrixXd Car::getStateJacobian(
const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const {

// Convert inputs to autodiff types
VectorXdual2nd state_dual = state.cast<autodiff::dual2nd>();
VectorXdual2nd control_dual = control.cast<autodiff::dual2nd>();

// Initialize Jacobian
Eigen::MatrixXd J = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM);

// Calculate Jacobian using autodiff
for (int i = 0; i < STATE_DIM; ++i) {
// Create a lambda that returns the i-th component of the dynamics
auto dynamics_i = [this, i, &control_dual, time](const VectorXdual2nd& x) -> autodiff::dual2nd {
VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(x, control_dual, time);
return dynamics(i);
};

// Calculate gradient of the i-th output with respect to state
J.row(i) = autodiff::gradient(dynamics_i, autodiff::wrt(state_dual), at(state_dual));
}

// Convert discrete Jacobian to continuous time Jacobian
J.diagonal().array() -= 1.0;
J /= timestep_;

Expand All @@ -87,54 +72,37 @@ Eigen::MatrixXd Car::getStateJacobian(

Eigen::MatrixXd Car::getControlJacobian(
const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const {

// Convert inputs to autodiff types
VectorXdual2nd state_dual = state.cast<autodiff::dual2nd>();
VectorXdual2nd control_dual = control.cast<autodiff::dual2nd>();

// Initialize Jacobian
Eigen::MatrixXd J = Eigen::MatrixXd::Zero(STATE_DIM, CONTROL_DIM);

// Calculate Jacobian using autodiff
for (int i = 0; i < STATE_DIM; ++i) {
// Create a lambda that returns the i-th component of the dynamics
auto dynamics_i = [this, i, &state_dual, time](const VectorXdual2nd& u) -> autodiff::dual2nd {
VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(state_dual, u, time);
return dynamics(i);
};

// Calculate gradient of the i-th output with respect to control
J.row(i) = autodiff::gradient(dynamics_i, autodiff::wrt(control_dual), at(control_dual));
}

// Convert discrete Jacobian to continuous time Jacobian
J /= timestep_;

return J;
}

std::vector<Eigen::MatrixXd> Car::getStateHessian(
const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const {

// Convert inputs to autodiff types
VectorXdual2nd state_dual = state.cast<autodiff::dual2nd>();
VectorXdual2nd control_dual = control.cast<autodiff::dual2nd>();

// Initialize Hessians
auto hessians = makeZeroTensor(STATE_DIM, STATE_DIM, STATE_DIM);

// Calculate Hessians using autodiff
for (int i = 0; i < STATE_DIM; ++i) {
// Create a lambda that returns the i-th component of the dynamics
auto dynamics_i = [this, i, &control_dual, time](const VectorXdual2nd& x) -> autodiff::dual2nd {
VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(x, control_dual, time);
return dynamics(i);
};

// Calculate Hessian of the i-th output with respect to state
hessians[i] = autodiff::hessian(dynamics_i, autodiff::wrt(state_dual), at(state_dual));

// Convert discrete Hessian to continuous time Hessian
hessians[i] /= timestep_;
}

Expand All @@ -143,50 +111,36 @@ std::vector<Eigen::MatrixXd> Car::getStateHessian(

std::vector<Eigen::MatrixXd> Car::getControlHessian(
const Eigen::VectorXd& state, const Eigen::VectorXd& control, double time) const {

// Convert inputs to autodiff types
VectorXdual2nd state_dual = state.cast<autodiff::dual2nd>();
VectorXdual2nd control_dual = control.cast<autodiff::dual2nd>();

// Initialize Hessians
auto hessians = makeZeroTensor(STATE_DIM, CONTROL_DIM, CONTROL_DIM);

// Calculate Hessians using autodiff
for (int i = 0; i < STATE_DIM; ++i) {
// Create a lambda that returns the i-th component of the dynamics
auto dynamics_i = [this, i, &state_dual, time](const VectorXdual2nd& u) -> autodiff::dual2nd {
VectorXdual2nd dynamics = this->getDiscreteDynamicsAutodiff(state_dual, u, time);
return dynamics(i);
};

// Calculate Hessian of the i-th output with respect to control
hessians[i] = autodiff::hessian(dynamics_i, autodiff::wrt(control_dual), at(control_dual));

// Convert discrete Hessian to continuous time Hessian
hessians[i] /= timestep_;
}

return hessians;
}

// Helper: Autodiff version of discrete dynamics
cddp::VectorXdual2nd Car::getDiscreteDynamicsAutodiff(
const VectorXdual2nd& state, const VectorXdual2nd& control, double time) const {

// Extract states (dual2nd)
const autodiff::dual2nd theta = state(STATE_THETA);
const autodiff::dual2nd v = state(STATE_V);

// Extract controls (dual2nd)
const autodiff::dual2nd delta = control(CONTROL_DELTA);
const autodiff::dual2nd a = control(CONTROL_A);

// Constants
const double d_double = wheelbase_;
const double h = timestep_;
const autodiff::dual2nd d = wheelbase_;

// Use ADL for math functions
const autodiff::dual2nd cos_theta = cos(theta);
const autodiff::dual2nd sin_theta = sin(theta);

Expand Down Expand Up @@ -228,4 +182,4 @@ cddp::VectorXdual2nd Car::getContinuousDynamicsAutodiff(
return (next_state - state) / timestep_;
}

} // namespace cddp
} // namespace cddp
Loading
Loading