Skip to content

feat(c3) : Add ability to evaluate cost of C3 plan #44

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

Open
ebianchi wants to merge 7 commits into
base: main
Choose a base branch
from
+814 −36
Open

feat(c3) : Add ability to evaluate cost of C3 plan #44

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
8f0deaf
Implement cost evaluation functionality; tests and examples still needed
ebianchi Feb 23, 2026
1daabf9
Initial attempt at python bindings (may be incomplete)
ebianchi Feb 23, 2026
b4743ae
Draft moving cost computation utilities outside C3 class
ebianchi Feb 25, 2026
df22ca5
Restructure to use namespace and static class functions
ebianchi Feb 26, 2026
16bc945
Add unit tests for trajectory evaluation functions
ebianchi Mar 2, 2026
c50db1a
Clean outdated comments; match function comment formatting to rest of…
ebianchi Mar 3, 2026
21f98bb
Switch quadratic trajectory cost implementation to more flexible vari…
ebianchi Mar 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
11 changes: 11 additions & 0 deletions core/BUILD.bazel
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,16 @@ cc_library(
],
)

cc_library(
name = "traj_eval",
srcs = ["traj_eval.cc"],
hdrs = ["traj_eval.h"],
deps = [
":c3",
"@drake//:drake_shared_library",
],
)

filegroup(
name = 'test_data',
srcs = glob(['test/resources/**'])
Expand All @@ -98,6 +108,7 @@ cc_test(
deps = [
":c3",
":c3_cartpole_problem",
":traj_eval",
"@gtest//:main",
],
env_inherit = [
Expand Down
66 changes: 32 additions & 34 deletions core/c3.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,10 +31,10 @@ using drake::solvers::OsqpSolver;
using drake::solvers::OsqpSolverDetails;
using drake::solvers::Solve;

C3::CostMatrices::CostMatrices(const std::vector<Eigen::MatrixXd>& Q,
const std::vector<Eigen::MatrixXd>& R,
const std::vector<Eigen::MatrixXd>& G,
const std::vector<Eigen::MatrixXd>& U) {
C3::CostMatrices::CostMatrices(const vector<MatrixXd>& Q,
const vector<MatrixXd>& R,
const vector<MatrixXd>& G,
const vector<MatrixXd>& U) {
this->Q = Q;
this->R = R;
this->G = G;
Expand Down Expand Up @@ -82,19 +82,19 @@ C3::C3(const LCS& lcs, const CostMatrices& costs,
u_ = vector<drake::solvers::VectorXDecisionVariable>();
lambda_ = vector<drake::solvers::VectorXDecisionVariable>();

z_sol_ = std::make_unique<std::vector<VectorXd>>();
x_sol_ = std::make_unique<std::vector<VectorXd>>();
lambda_sol_ = std::make_unique<std::vector<VectorXd>>();
u_sol_ = std::make_unique<std::vector<VectorXd>>();
w_sol_ = std::make_unique<std::vector<VectorXd>>();
delta_sol_ = std::make_unique<std::vector<VectorXd>>();
z_sol_ = std::make_unique<vector<VectorXd>>();
x_sol_ = std::make_unique<vector<VectorXd>>();
lambda_sol_ = std::make_unique<vector<VectorXd>>();
u_sol_ = std::make_unique<vector<VectorXd>>();
w_sol_ = std::make_unique<vector<VectorXd>>();
delta_sol_ = std::make_unique<vector<VectorXd>>();
for (int i = 0; i < N_; ++i) {
x_sol_->push_back(Eigen::VectorXd::Zero(n_x_));
lambda_sol_->push_back(Eigen::VectorXd::Zero(n_lambda_));
u_sol_->push_back(Eigen::VectorXd::Zero(n_u_));
z_sol_->push_back(Eigen::VectorXd::Zero(n_z_));
w_sol_->push_back(Eigen::VectorXd::Zero(n_z_));
delta_sol_->push_back(Eigen::VectorXd::Zero(n_z_));
x_sol_->push_back(VectorXd::Zero(n_x_));
lambda_sol_->push_back(VectorXd::Zero(n_lambda_));
u_sol_->push_back(VectorXd::Zero(n_u_));
z_sol_->push_back(VectorXd::Zero(n_z_));
w_sol_->push_back(VectorXd::Zero(n_z_));
delta_sol_->push_back(VectorXd::Zero(n_z_));
}

z_.resize(N_);
Expand Down Expand Up @@ -183,13 +183,11 @@ C3::C3(const LCS& lcs, const CostMatrices& costs,

C3::CostMatrices C3::CreateCostMatricesFromC3Options(const C3Options& options,
int N) {
std::vector<Eigen::MatrixXd> Q; // State cost matrices.
std::vector<Eigen::MatrixXd> R; // Input cost matrices.
vector<MatrixXd> Q; // State cost matrices.
vector<MatrixXd> R; // Input cost matrices.

std::vector<MatrixXd> G(N,
options.G); // State-input cross-term matrices.
std::vector<MatrixXd> U(N,
options.U); // Constraint matrices.
vector<MatrixXd> G(N, options.G); // State-input cross-term matrices.
vector<MatrixXd> U(N, options.U); // Constraint matrices.

double discount_factor = 1.0;
for (int i = 0; i < N; ++i) {
Expand Down Expand Up @@ -232,12 +230,12 @@ void C3::UpdateLCS(const LCS& lcs) {
}
}

const std::vector<drake::solvers::LinearEqualityConstraint*>&
const vector<drake::solvers::LinearEqualityConstraint*>&
C3::GetDynamicConstraints() {
return dynamics_constraints_;
}

void C3::UpdateTarget(const std::vector<Eigen::VectorXd>& x_des) {
void C3::UpdateTarget(const vector<VectorXd>& x_des) {
x_desired_ = x_des;
for (int i = 0; i < N_ + 1; ++i) {
target_costs_[i]->UpdateCoefficients(
Expand All @@ -261,7 +259,7 @@ void C3::UpdateCostMatrices(const CostMatrices& costs) {
}
}

const std::vector<drake::solvers::QuadraticCost*>& C3::GetTargetCost() {
const vector<drake::solvers::QuadraticCost*>& C3::GetTargetCost() {
return target_costs_;
}

Expand All @@ -285,7 +283,8 @@ void C3::Solve(const VectorXd& x0) {
VectorXd lambda0;
LCPSolver.SolveLcpLemke(lcs_.F()[0], lcs_.E()[0] * x0 + lcs_.c()[0],
&lambda0);
// Force constraints to be updated before every solve if no dependence on u
// Force constraints to be updated before every solve if no dependence on
// u
if (initial_force_constraint_) {
initial_force_constraint_->UpdateCoefficients(
MatrixXd::Identity(n_lambda_, n_lambda_), lambda0);
Expand All @@ -312,8 +311,8 @@ void C3::Solve(const VectorXd& x0) {
if (options_.delta_option == 1) {
delta_init.head(n_x_) = x0;
}
std::vector<VectorXd> delta(N_, delta_init);
std::vector<VectorXd> w(N_, VectorXd::Zero(n_z_));
vector<VectorXd> delta(N_, delta_init);
vector<VectorXd> w(N_, VectorXd::Zero(n_z_));
vector<MatrixXd> G = cost_matrices_.G;

for (int iter = 0; iter < options_.admm_iter; iter++) {
Expand Down Expand Up @@ -385,7 +384,7 @@ void C3::ADMMStep(const VectorXd& x0, vector<VectorXd>* delta,
}
}

void C3::SetInitialGuessQP(const Eigen::VectorXd& x0, int admm_iteration) {
void C3::SetInitialGuessQP(const VectorXd& x0, int admm_iteration) {
prog_.SetInitialGuess(x_[0], x0);
if (!warm_start_ || admm_iteration == 0)
return; // No warm start for the first iteration
Expand Down Expand Up @@ -501,8 +500,7 @@ vector<VectorXd> C3::SolveProjection(const vector<MatrixXd>& U,
return deltaProj;
}

void C3::AddLinearConstraint(const Eigen::MatrixXd& A,
const VectorXd& lower_bound,
void C3::AddLinearConstraint(const MatrixXd& A, const VectorXd& lower_bound,
const VectorXd& upper_bound,
ConstraintVariable constraint) {
if (constraint == 1) {
Expand Down Expand Up @@ -530,9 +528,9 @@ void C3::AddLinearConstraint(const Eigen::MatrixXd& A,
void C3::AddLinearConstraint(const Eigen::RowVectorXd& A, double lower_bound,
double upper_bound,
ConstraintVariable constraint) {
Eigen::VectorXd lb(1);
VectorXd lb(1);
lb << lower_bound;
Eigen::VectorXd ub(1);
VectorXd ub(1);
ub << upper_bound;
AddLinearConstraint(A, lb, ub, constraint);
}
Expand All @@ -544,7 +542,7 @@ void C3::RemoveConstraints() {
user_constraints_.clear();
}

const std::vector<LinearConstraintBinding>& C3::GetLinearConstraints() {
const vector<LinearConstraintBinding>& C3::GetLinearConstraints() {
return user_constraints_;
}

Expand Down
2 changes: 2 additions & 0 deletions core/c3.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,8 @@ class C3 {
std::vector<Eigen::MatrixXd> R;
std::vector<Eigen::MatrixXd> G;
std::vector<Eigen::MatrixXd> U;
std::vector<Eigen::MatrixXd> Q_evaluation;
std::vector<Eigen::MatrixXd> R_evaluation;
};

/*!
Expand Down
2 changes: 1 addition & 1 deletion core/lcs.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ double LCS::ScaleComplementarityDynamics() {
return scale;
}

const VectorXd LCS::Simulate(VectorXd& x_init, VectorXd& u,
const VectorXd LCS::Simulate(const VectorXd& x_init, const VectorXd& u,
const LCSSimulateConfig& config) const {
VectorXd x_final;
VectorXd force;
Expand Down
2 changes: 1 addition & 1 deletion core/lcs.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ class LCS {
* @return The state at the next timestep
*/
const Eigen::VectorXd Simulate(
Eigen::VectorXd& x_init, Eigen::VectorXd& u,
const Eigen::VectorXd& x_init, const Eigen::VectorXd& u,
const LCSSimulateConfig& config = LCSSimulateConfig()) const;

/*!
Expand Down
Loading