From 8f0deafc79dd6f878790a89609b7d5e18a064ee4 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Mon, 23 Feb 2026 17:23:42 -0500 Subject: [PATCH 01/17] Implement cost evaluation functionality; tests and examples still needed --- core/c3.cc | 192 +++++++++++++++++++++++++++++++++++++++++++---------- core/c3.h | 53 ++++++++++++++- 2 files changed, 209 insertions(+), 36 deletions(-) diff --git a/core/c3.cc b/core/c3.cc index f46c20a3..e52ff8df 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -31,14 +31,26 @@ using drake::solvers::OsqpSolver; using drake::solvers::OsqpSolverDetails; using drake::solvers::Solve; -C3::CostMatrices::CostMatrices(const std::vector& Q, - const std::vector& R, - const std::vector& G, - const std::vector& U) { +C3::CostMatrices::CostMatrices(const vector& Q, + const vector& R, + const vector& G, + const vector& U, + const vector& Q_evaluation, + const vector& R_evaluation) { this->Q = Q; this->R = R; this->G = G; this->U = U; + if (Q_evaluation.empty()) { + this->Q_evaluation = Q; + } else { + this->Q_evaluation = Q_evaluation; + } + if (R_evaluation.empty()) { + this->R_evaluation = R; + } else { + this->R_evaluation = R_evaluation; + } } C3::C3(const LCS& lcs, const CostMatrices& costs, @@ -51,6 +63,7 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, n_u_(lcs.num_inputs()), n_z_(z_size), lcs_(lcs), + cost_lcs_(lcs), cost_matrices_(costs), x_desired_(x_desired), options_(options), @@ -82,19 +95,19 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, u_ = vector(); lambda_ = vector(); - z_sol_ = std::make_unique>(); - x_sol_ = std::make_unique>(); - lambda_sol_ = std::make_unique>(); - u_sol_ = std::make_unique>(); - w_sol_ = std::make_unique>(); - delta_sol_ = std::make_unique>(); + z_sol_ = std::make_unique>(); + x_sol_ = std::make_unique>(); + lambda_sol_ = std::make_unique>(); + u_sol_ = std::make_unique>(); + w_sol_ = std::make_unique>(); + delta_sol_ = std::make_unique>(); 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_); @@ -183,13 +196,11 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, C3::CostMatrices C3::CreateCostMatricesFromC3Options(const C3Options& options, int N) { - std::vector Q; // State cost matrices. - std::vector R; // Input cost matrices. + vector Q; // State cost matrices. + vector R; // Input cost matrices. - std::vector G(N, - options.G); // State-input cross-term matrices. - std::vector U(N, - options.U); // Constraint matrices. + vector G(N, options.G); // State-input cross-term matrices. + vector U(N, options.U); // Constraint matrices. double discount_factor = 1.0; for (int i = 0; i < N; ++i) { @@ -213,6 +224,119 @@ void C3::ScaleLCS() { AnDn_ = lcs_.ScaleComplementarityDynamics(); } +void C3::UpdateCostLCS(const LCS& cost_lcs) { + DRAKE_DEMAND(lcs_.num_states() == cost_lcs.num_states()); + DRAKE_DEMAND(lcs_.num_inputs() == cost_lcs.num_inputs()); + DRAKE_DEMAND(lcs_.N() <= cost_lcs.N()); + DRAKE_DEMAND(lcs_.dt() >= cost_lcs.dt()); + DRAKE_DEMAND(lcs_.N() * lcs_.dt() == cost_lcs.N() * cost_lcs.dt()); + + int timestep_split = cost_lcs.N() / lcs_.N(); + DRAKE_DEMAND(cost_lcs.dt() * timestep_split == lcs_.dt()); + DRAKE_DEMAND(lcs_.N() * timestep_split == cost_lcs.N()); + + // Update the stored LCS object. + cost_lcs_ = cost_lcs; +} + +std::pair> C3::CalculateCost( + const CostComputationType& cost_type, const VectorXd& Kp, + const VectorXd& Kd) { + DRAKE_DEMAND(cost_type == CostComputationType::SIM_IMPEDANCE || + cost_type == CostComputationType::SIM_OBJECT_LCS_ROBOT_PLAN); + + int timestep_split = cost_lcs_.N() / lcs_.N(); + + // Get the C3 plan. + vector planned_state_trajectory_coarse = GetStateSolution(); + vector planned_input_trajectory_coarse = GetInputSolution(); + + // Compute the new trajectory used for cost evaluation. + vector state_trajectory(N_ * timestep_split + 1, + VectorXd::Zero(n_x_)); + vector input_trajectory(N_ * timestep_split, VectorXd::Zero(n_u_)); + if (cost_type == CostComputationType::SIM_IMPEDANCE) { + // Ensure the Kp and Kd vectors encode the actuated position and velocity + // indices within the state vector. + DRAKE_DEMAND(Kp.rows() == Kd.rows() && Kp.rows() == n_x_); + DRAKE_DEMAND((Kp.array() != 0.0).count() == n_u_); + DRAKE_DEMAND((Kd.array() != 0.0).count() == n_u_); + MatrixXd Kp_mat = MatrixXd::Zero(n_u_, n_x_); + MatrixXd Kd_mat = MatrixXd::Zero(n_u_, n_x_); + int kp_i = 0; + int kd_i = 0; + for (int i = 0; i < n_x_; ++i) { + if (Kp(i) != 0) { + Kp_mat(kp_i, i) = Kp(i); + kp_i++; + } + if (Kd(i) != 0) { + Kd_mat(kd_i, i) = Kd(i); + kd_i++; + } + } + + state_trajectory[0] = planned_state_trajectory_coarse[0]; + for (int i = 0; i < N_ * timestep_split; i++) { + VectorXd x = planned_state_trajectory_coarse.at(i / timestep_split); + VectorXd u = planned_input_trajectory_coarse.at(i / timestep_split); + VectorXd x_des = x_desired_.at(i / timestep_split); + VectorXd x_curr = state_trajectory.at(i); + input_trajectory[i] = + planned_input_trajectory_coarse.at(i / timestep_split); + input_trajectory[i] += Kp_mat * (x - x_curr) + Kd_mat * (x - x_curr); + state_trajectory[i + 1] = cost_lcs_.Simulate(x_curr, input_trajectory[i]); + } + } else if (cost_type == CostComputationType::SIM_OBJECT_LCS_ROBOT_PLAN) { + state_trajectory[0] = planned_state_trajectory_coarse[0]; + for (int i = 0; i < N_ * timestep_split; i++) { + VectorXd x_curr = state_trajectory.at(i); + input_trajectory[i] = + planned_input_trajectory_coarse.at(i / timestep_split); + state_trajectory[i + 1] = cost_lcs_.Simulate(x_curr, input_trajectory[i]); + } + // Replace the simulated robot states with the planned ones. + for (int i = 0; i < N_ * timestep_split; i++) { + state_trajectory[i].segment(0, n_u_) = + planned_state_trajectory_coarse.at(i / timestep_split) + .segment(0, n_u_); + } + state_trajectory[N_ * timestep_split].segment(0, n_u_) = + cost_lcs_ + .Simulate(state_trajectory[N_ * timestep_split - 1], + input_trajectory[N_ * timestep_split - 1]) + .segment(0, n_u_); + } + + // Downsample the state trajectory to match the C3 plan timesteps. + vector state_trajectory_downsampled(N_ + 1, VectorXd::Zero(n_x_)); + vector input_trajectory_downsampled(N_, VectorXd::Zero(n_u_)); + for (int i = 0; i < N_ * timestep_split; i += timestep_split) { + state_trajectory_downsampled[i / timestep_split] = state_trajectory[i]; + input_trajectory_downsampled[i / timestep_split] = input_trajectory[i]; + } + state_trajectory_downsampled[N_] = state_trajectory[N_ * timestep_split]; + + // Compute the cost based on the downsampled trajectory. + // TODO @bibit: doesn't handle (u-u_prev)^T R (u-u_prev) + double cost = 0.0; + for (int i = 0; i < N_ + 1; i++) { + VectorXd x_des = x_desired_.at(i); + cost += (state_trajectory_downsampled[i] - x_des).transpose() * + cost_matrices_.Q_evaluation.at(i) * + (state_trajectory_downsampled[i] - x_des); + if (i < N_) { + cost += input_trajectory_downsampled[i].transpose() * + cost_matrices_.R_evaluation.at(i) * + input_trajectory_downsampled[i]; + } + } + + std::pair> cost_and_trajectory( + cost, state_trajectory_downsampled); + return cost_and_trajectory; +} + void C3::UpdateLCS(const LCS& lcs) { DRAKE_DEMAND(lcs_.HasSameDimensionsAs(lcs)); @@ -232,12 +356,12 @@ void C3::UpdateLCS(const LCS& lcs) { } } -const std::vector& +const vector& C3::GetDynamicConstraints() { return dynamics_constraints_; } -void C3::UpdateTarget(const std::vector& x_des) { +void C3::UpdateTarget(const vector& x_des) { x_desired_ = x_des; for (int i = 0; i < N_ + 1; ++i) { target_costs_[i]->UpdateCoefficients( @@ -261,7 +385,7 @@ void C3::UpdateCostMatrices(const CostMatrices& costs) { } } -const std::vector& C3::GetTargetCost() { +const vector& C3::GetTargetCost() { return target_costs_; } @@ -285,7 +409,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); @@ -312,8 +437,8 @@ void C3::Solve(const VectorXd& x0) { if (options_.delta_option == 1) { delta_init.head(n_x_) = x0; } - std::vector delta(N_, delta_init); - std::vector w(N_, VectorXd::Zero(n_z_)); + vector delta(N_, delta_init); + vector w(N_, VectorXd::Zero(n_z_)); vector G = cost_matrices_.G; for (int iter = 0; iter < options_.admm_iter; iter++) { @@ -385,7 +510,7 @@ void C3::ADMMStep(const VectorXd& x0, vector* 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 @@ -501,8 +626,7 @@ vector C3::SolveProjection(const vector& 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) { @@ -530,9 +654,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); } @@ -544,7 +668,7 @@ void C3::RemoveConstraints() { user_constraints_.clear(); } -const std::vector& C3::GetLinearConstraints() { +const vector& C3::GetLinearConstraints() { return user_constraints_; } diff --git a/core/c3.h b/core/c3.h index 1fe98ff1..fe4e679a 100644 --- a/core/c3.h +++ b/core/c3.h @@ -18,6 +18,11 @@ typedef drake::solvers::Binding enum ConstraintVariable : uint8_t { STATE = 1, INPUT = 2, FORCE = 3 }; +enum CostComputationType : uint8_t { + SIM_IMPEDANCE = 1, + SIM_OBJECT_LCS_ROBOT_PLAN = 2 +}; + class C3 { public: /*! @@ -30,7 +35,9 @@ class C3 { CostMatrices(const std::vector& Q, const std::vector& R, const std::vector& G, - const std::vector& U); + const std::vector& U, + const std::vector& Q_evaluation = {}, + const std::vector& R_evaluation = {}); bool HasSameDimensionsAs(const CostMatrices& other) const { // Check vector and matrix dimensions return (Q.size() == other.Q.size() && @@ -44,12 +51,20 @@ class C3 { G.at(0).cols() == other.G.at(0).cols() && U.size() == other.U.size() && U.at(0).rows() == other.U.at(0).rows() && - U.at(0).cols() == other.U.at(0).cols()); + U.at(0).cols() == other.U.at(0).cols() && + Q_evaluation.size() == other.Q_evaluation.size() && + Q_evaluation.at(0).rows() == other.Q_evaluation.at(0).rows() && + Q_evaluation.at(0).cols() == other.Q_evaluation.at(0).cols() && + R_evaluation.size() == other.R_evaluation.size() && + R_evaluation.at(0).rows() == other.R_evaluation.at(0).rows() && + R_evaluation.at(0).cols() == other.R_evaluation.at(0).cols()); } std::vector Q; std::vector R; std::vector G; std::vector U; + std::vector Q_evaluation; + std::vector R_evaluation; }; /*! @@ -189,6 +204,39 @@ class C3 { prog_.SetSolverOptions(options); } + /** + * @brief Update the cost LCS used for cost evaluation in CalculateCost. + * + * @param cost_lcs the new LCS to be used for cost evaluation. Must have the + * same state and input dimensions as the LCS used for planning. The horizon + * and timestep can differ, but must be compatible in that cost_lcs.N() must + * be a multiple of lcs_.N(), and N*dt must be the same for both LCS objects. + * This allows for evaluating costs with a higher temporal resolution than the + * planning LCS. Can differ in lambda dimensions, if higher contact resolution + * is desired for evaluation. + */ + void UpdateCostLCS(const LCS& cost_lcs); + + /** + * @brief Calculate the cost of the current solution, using the provided cost + * type and optionally provided Kp and Kd values for the cost calculation. + * + * @param cost_type + * @param Kp Proportional gain values for the cost calculation, used only if + * the cost type requires them. If used, the length of Kp should be n_x_ and + * the number of non-zero elements should be n_u_. This encodes which state + * indices are positions associated with a robot actuator. + * @param Kd Derivative gain values for the cost calculation, used only if + * the cost type requires them. If used, the length of Kd should be n_x_ and + * the number of non-zero elements should be n_u_. This encodes which state + * indices are velocities associated with a robot actuator. + * @return A pair consisting of the cost (as a double) and a vector of the + * state trajectory associated with the cost. + */ + std::pair> CalculateCost( + const CostComputationType& cost_type, const Eigen::VectorXd& Kp = {}, + const Eigen::VectorXd& Kd = {}); + std::vector GetFullSolution() { return *z_sol_; } std::vector GetStateSolution() { return *x_sol_; } std::vector GetForceSolution() { return *lambda_sol_; } @@ -388,6 +436,7 @@ class C3 { int admm_iteration, bool is_final_solve); LCS lcs_; + LCS cost_lcs_; double AnDn_ = 1.0; // Scaling factor for lambdas CostMatrices cost_matrices_; std::vector x_desired_; From 1daabf9075124e09e436d27c6928152faded1ae4 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Mon, 23 Feb 2026 17:39:38 -0500 Subject: [PATCH 02/17] Initial attempt at python bindings (may be incomplete) --- bindings/pyc3/c3_py.cc | 8 ++++++++ core/c3.cc | 2 +- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/bindings/pyc3/c3_py.cc b/bindings/pyc3/c3_py.cc index 703c58f6..f669e840 100644 --- a/bindings/pyc3/c3_py.cc +++ b/bindings/pyc3/c3_py.cc @@ -73,6 +73,11 @@ PYBIND11_MODULE(c3, m) { .value("FORCE", ConstraintVariable::FORCE) .export_values(); + py::enum_(m, "CostComputationType") + .value("STATE", CostComputationType::SIM_IMPEDANCE) + .value("INPUT", CostComputationType::SIM_OBJECT_LCS_ROBOT_PLAN) + .export_values(); + py::class_(m, "C3") .def(py::init&, const C3Options&>(), @@ -110,6 +115,9 @@ PYBIND11_MODULE(c3, m) { .def("GetLinearConstraints", &C3::GetLinearConstraints, py::return_value_policy::copy) .def("SetSolverOptions", &C3::SetSolverOptions, py::arg("options")) + .def("UpdateCostLCS", &C3::UpdateCostLCS, py::arg("cost_lcs")) + .def("CalculateCost", &C3::CalculateCost, py::arg("cost_type"), + py::arg("Kp"), py::arg("Kd")) .def("GetFullSolution", &C3::GetFullSolution) .def("GetStateSolution", &C3::GetStateSolution) .def("GetForceSolution", &C3::GetForceSolution) diff --git a/core/c3.cc b/core/c3.cc index e52ff8df..e04413f9 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -318,7 +318,7 @@ std::pair> C3::CalculateCost( state_trajectory_downsampled[N_] = state_trajectory[N_ * timestep_split]; // Compute the cost based on the downsampled trajectory. - // TODO @bibit: doesn't handle (u-u_prev)^T R (u-u_prev) + // NOTE: this doesn't handle (u-u_prev)^T R (u-u_prev) double cost = 0.0; for (int i = 0; i < N_ + 1; i++) { VectorXd x_des = x_desired_.at(i); From b4743aef34bc4714a46fb4f06dd4ca00038e6b9b Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Wed, 25 Feb 2026 14:34:26 -0500 Subject: [PATCH 03/17] Draft moving cost computation utilities outside C3 class --- bindings/pyc3/c3_py.cc | 14 +- core/BUILD.bazel | 10 ++ core/c3.cc | 232 ++++++++++++------------- core/c3.h | 33 ++-- core/lcs.cc | 2 +- core/lcs.h | 2 +- core/test/core_test.cc | 1 + core/trajectory_evaluation.cc | 310 ++++++++++++++++++++++++++++++++++ core/trajectory_evaluation.h | 160 ++++++++++++++++++ 9 files changed, 615 insertions(+), 149 deletions(-) create mode 100644 core/trajectory_evaluation.cc create mode 100644 core/trajectory_evaluation.h diff --git a/bindings/pyc3/c3_py.cc b/bindings/pyc3/c3_py.cc index f669e840..99ee8e25 100644 --- a/bindings/pyc3/c3_py.cc +++ b/bindings/pyc3/c3_py.cc @@ -73,10 +73,10 @@ PYBIND11_MODULE(c3, m) { .value("FORCE", ConstraintVariable::FORCE) .export_values(); - py::enum_(m, "CostComputationType") - .value("STATE", CostComputationType::SIM_IMPEDANCE) - .value("INPUT", CostComputationType::SIM_OBJECT_LCS_ROBOT_PLAN) - .export_values(); + // py::enum_(m, "CostComputationType") + // .value("STATE", CostComputationType::SIM_IMPEDANCE) + // .value("INPUT", CostComputationType::SIM_OBJECT_LCS_ROBOT_PLAN) + // .export_values(); // TODO @bibit: remove py::class_(m, "C3") .def(py::init& Q, const vector& R, const vector& G, - const vector& U, - const vector& Q_evaluation, - const vector& R_evaluation) { + const vector& U) { this->Q = Q; this->R = R; this->G = G; this->U = U; - if (Q_evaluation.empty()) { - this->Q_evaluation = Q; - } else { - this->Q_evaluation = Q_evaluation; - } - if (R_evaluation.empty()) { - this->R_evaluation = R; - } else { - this->R_evaluation = R_evaluation; - } } C3::C3(const LCS& lcs, const CostMatrices& costs, @@ -63,7 +51,7 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, n_u_(lcs.num_inputs()), n_z_(z_size), lcs_(lcs), - cost_lcs_(lcs), + // cost_lcs_(lcs), // TODO @bibit: remove cost_matrices_(costs), x_desired_(x_desired), options_(options), @@ -224,118 +212,120 @@ void C3::ScaleLCS() { AnDn_ = lcs_.ScaleComplementarityDynamics(); } +/* TODO @bibit: remove void C3::UpdateCostLCS(const LCS& cost_lcs) { - DRAKE_DEMAND(lcs_.num_states() == cost_lcs.num_states()); - DRAKE_DEMAND(lcs_.num_inputs() == cost_lcs.num_inputs()); - DRAKE_DEMAND(lcs_.N() <= cost_lcs.N()); - DRAKE_DEMAND(lcs_.dt() >= cost_lcs.dt()); - DRAKE_DEMAND(lcs_.N() * lcs_.dt() == cost_lcs.N() * cost_lcs.dt()); - - int timestep_split = cost_lcs.N() / lcs_.N(); - DRAKE_DEMAND(cost_lcs.dt() * timestep_split == lcs_.dt()); - DRAKE_DEMAND(lcs_.N() * timestep_split == cost_lcs.N()); - - // Update the stored LCS object. - cost_lcs_ = cost_lcs; + DRAKE_DEMAND(lcs_.num_states() == cost_lcs.num_states()); + DRAKE_DEMAND(lcs_.num_inputs() == cost_lcs.num_inputs()); + DRAKE_DEMAND(lcs_.N() <= cost_lcs.N()); + DRAKE_DEMAND(lcs_.dt() >= cost_lcs.dt()); + DRAKE_DEMAND(lcs_.N() * lcs_.dt() == cost_lcs.N() * cost_lcs.dt()); + + int timestep_split = cost_lcs.N() / lcs_.N(); + DRAKE_DEMAND(cost_lcs.dt() * timestep_split == lcs_.dt()); + DRAKE_DEMAND(lcs_.N() * timestep_split == cost_lcs.N()); + + // Update the stored LCS object. + cost_lcs_ = cost_lcs; } std::pair> C3::CalculateCost( - const CostComputationType& cost_type, const VectorXd& Kp, - const VectorXd& Kd) { - DRAKE_DEMAND(cost_type == CostComputationType::SIM_IMPEDANCE || - cost_type == CostComputationType::SIM_OBJECT_LCS_ROBOT_PLAN); - - int timestep_split = cost_lcs_.N() / lcs_.N(); - - // Get the C3 plan. - vector planned_state_trajectory_coarse = GetStateSolution(); - vector planned_input_trajectory_coarse = GetInputSolution(); - - // Compute the new trajectory used for cost evaluation. - vector state_trajectory(N_ * timestep_split + 1, - VectorXd::Zero(n_x_)); - vector input_trajectory(N_ * timestep_split, VectorXd::Zero(n_u_)); - if (cost_type == CostComputationType::SIM_IMPEDANCE) { - // Ensure the Kp and Kd vectors encode the actuated position and velocity - // indices within the state vector. - DRAKE_DEMAND(Kp.rows() == Kd.rows() && Kp.rows() == n_x_); - DRAKE_DEMAND((Kp.array() != 0.0).count() == n_u_); - DRAKE_DEMAND((Kd.array() != 0.0).count() == n_u_); - MatrixXd Kp_mat = MatrixXd::Zero(n_u_, n_x_); - MatrixXd Kd_mat = MatrixXd::Zero(n_u_, n_x_); - int kp_i = 0; - int kd_i = 0; - for (int i = 0; i < n_x_; ++i) { - if (Kp(i) != 0) { - Kp_mat(kp_i, i) = Kp(i); - kp_i++; - } - if (Kd(i) != 0) { - Kd_mat(kd_i, i) = Kd(i); - kd_i++; - } - } - - state_trajectory[0] = planned_state_trajectory_coarse[0]; - for (int i = 0; i < N_ * timestep_split; i++) { - VectorXd x = planned_state_trajectory_coarse.at(i / timestep_split); - VectorXd u = planned_input_trajectory_coarse.at(i / timestep_split); - VectorXd x_des = x_desired_.at(i / timestep_split); - VectorXd x_curr = state_trajectory.at(i); - input_trajectory[i] = - planned_input_trajectory_coarse.at(i / timestep_split); - input_trajectory[i] += Kp_mat * (x - x_curr) + Kd_mat * (x - x_curr); - state_trajectory[i + 1] = cost_lcs_.Simulate(x_curr, input_trajectory[i]); - } - } else if (cost_type == CostComputationType::SIM_OBJECT_LCS_ROBOT_PLAN) { - state_trajectory[0] = planned_state_trajectory_coarse[0]; - for (int i = 0; i < N_ * timestep_split; i++) { - VectorXd x_curr = state_trajectory.at(i); - input_trajectory[i] = - planned_input_trajectory_coarse.at(i / timestep_split); - state_trajectory[i + 1] = cost_lcs_.Simulate(x_curr, input_trajectory[i]); - } - // Replace the simulated robot states with the planned ones. - for (int i = 0; i < N_ * timestep_split; i++) { - state_trajectory[i].segment(0, n_u_) = - planned_state_trajectory_coarse.at(i / timestep_split) - .segment(0, n_u_); - } - state_trajectory[N_ * timestep_split].segment(0, n_u_) = - cost_lcs_ - .Simulate(state_trajectory[N_ * timestep_split - 1], - input_trajectory[N_ * timestep_split - 1]) - .segment(0, n_u_); - } - - // Downsample the state trajectory to match the C3 plan timesteps. - vector state_trajectory_downsampled(N_ + 1, VectorXd::Zero(n_x_)); - vector input_trajectory_downsampled(N_, VectorXd::Zero(n_u_)); - for (int i = 0; i < N_ * timestep_split; i += timestep_split) { - state_trajectory_downsampled[i / timestep_split] = state_trajectory[i]; - input_trajectory_downsampled[i / timestep_split] = input_trajectory[i]; - } - state_trajectory_downsampled[N_] = state_trajectory[N_ * timestep_split]; - - // Compute the cost based on the downsampled trajectory. - // NOTE: this doesn't handle (u-u_prev)^T R (u-u_prev) - double cost = 0.0; - for (int i = 0; i < N_ + 1; i++) { - VectorXd x_des = x_desired_.at(i); - cost += (state_trajectory_downsampled[i] - x_des).transpose() * - cost_matrices_.Q_evaluation.at(i) * - (state_trajectory_downsampled[i] - x_des); - if (i < N_) { - cost += input_trajectory_downsampled[i].transpose() * - cost_matrices_.R_evaluation.at(i) * - input_trajectory_downsampled[i]; - } - } - - std::pair> cost_and_trajectory( - cost, state_trajectory_downsampled); - return cost_and_trajectory; + const CostComputationType& cost_type, const VectorXd& Kp, + const VectorXd& Kd) { + DRAKE_DEMAND(cost_type == CostComputationType::SIM_IMPEDANCE || + cost_type == CostComputationType::SIM_OBJECT_LCS_ROBOT_PLAN); + + int timestep_split = cost_lcs_.N() / lcs_.N(); + + // Get the C3 plan. + vector planned_state_trajectory_coarse = GetStateSolution(); + vector planned_input_trajectory_coarse = GetInputSolution(); + + // Compute the new trajectory used for cost evaluation. + vector state_trajectory(N_ * timestep_split + 1, + VectorXd::Zero(n_x_)); + vector input_trajectory(N_ * timestep_split, VectorXd::Zero(n_u_)); + if (cost_type == CostComputationType::SIM_IMPEDANCE) { + // Ensure the Kp and Kd vectors encode the actuated position and velocity + // indices within the state vector. + DRAKE_DEMAND(Kp.rows() == Kd.rows() && Kp.rows() == n_x_); + DRAKE_DEMAND((Kp.array() != 0.0).count() == n_u_); + DRAKE_DEMAND((Kd.array() != 0.0).count() == n_u_); + MatrixXd Kp_mat = MatrixXd::Zero(n_u_, n_x_); + MatrixXd Kd_mat = MatrixXd::Zero(n_u_, n_x_); + int kp_i = 0; + int kd_i = 0; + for (int i = 0; i < n_x_; ++i) { + if (Kp(i) != 0) { + Kp_mat(kp_i, i) = Kp(i); + kp_i++; + } + if (Kd(i) != 0) { + Kd_mat(kd_i, i) = Kd(i); + kd_i++; + } + } + + state_trajectory[0] = planned_state_trajectory_coarse[0]; + for (int i = 0; i < N_ * timestep_split; i++) { + VectorXd x = planned_state_trajectory_coarse.at(i / timestep_split); + VectorXd u = planned_input_trajectory_coarse.at(i / timestep_split); + VectorXd x_des = x_desired_.at(i / timestep_split); + VectorXd x_curr = state_trajectory.at(i); + input_trajectory[i] = + planned_input_trajectory_coarse.at(i / timestep_split); + input_trajectory[i] += Kp_mat * (x - x_curr) + Kd_mat * (x - x_curr); + state_trajectory[i + 1] = cost_lcs_.Simulate(x_curr, input_trajectory[i]); + } + } else if (cost_type == CostComputationType::SIM_OBJECT_LCS_ROBOT_PLAN) { + state_trajectory[0] = planned_state_trajectory_coarse[0]; + for (int i = 0; i < N_ * timestep_split; i++) { + VectorXd x_curr = state_trajectory.at(i); + input_trajectory[i] = + planned_input_trajectory_coarse.at(i / timestep_split); + state_trajectory[i + 1] = cost_lcs_.Simulate(x_curr, input_trajectory[i]); + } + // Replace the simulated robot states with the planned ones. + for (int i = 0; i < N_ * timestep_split; i++) { + state_trajectory[i].segment(0, n_u_) = + planned_state_trajectory_coarse.at(i / timestep_split) + .segment(0, n_u_); + } + state_trajectory[N_ * timestep_split].segment(0, n_u_) = + cost_lcs_ + .Simulate(state_trajectory[N_ * timestep_split - 1], + input_trajectory[N_ * timestep_split - 1]) + .segment(0, n_u_); + } + + // Downsample the state trajectory to match the C3 plan timesteps. + vector state_trajectory_downsampled(N_ + 1, VectorXd::Zero(n_x_)); + vector input_trajectory_downsampled(N_, VectorXd::Zero(n_u_)); + for (int i = 0; i < N_ * timestep_split; i += timestep_split) { + state_trajectory_downsampled[i / timestep_split] = state_trajectory[i]; + input_trajectory_downsampled[i / timestep_split] = input_trajectory[i]; + } + state_trajectory_downsampled[N_] = state_trajectory[N_ * timestep_split]; + + // Compute the cost based on the downsampled trajectory. + // NOTE: this doesn't handle (u-u_prev)^T R (u-u_prev) + double cost = 0.0; + for (int i = 0; i < N_ + 1; i++) { + VectorXd x_des = x_desired_.at(i); + cost += (state_trajectory_downsampled[i] - x_des).transpose() * + cost_matrices_.Q_evaluation.at(i) * + (state_trajectory_downsampled[i] - x_des); + if (i < N_) { + cost += input_trajectory_downsampled[i].transpose() * + cost_matrices_.R_evaluation.at(i) * + input_trajectory_downsampled[i]; + } + } + + std::pair> cost_and_trajectory( + cost, state_trajectory_downsampled); + return cost_and_trajectory; } +*/ void C3::UpdateLCS(const LCS& lcs) { DRAKE_DEMAND(lcs_.HasSameDimensionsAs(lcs)); diff --git a/core/c3.h b/core/c3.h index fe4e679a..2a083063 100644 --- a/core/c3.h +++ b/core/c3.h @@ -18,10 +18,11 @@ typedef drake::solvers::Binding enum ConstraintVariable : uint8_t { STATE = 1, INPUT = 2, FORCE = 3 }; -enum CostComputationType : uint8_t { - SIM_IMPEDANCE = 1, - SIM_OBJECT_LCS_ROBOT_PLAN = 2 -}; +// TODO @bibit: remove +// enum CostComputationType : uint8_t { +// SIM_IMPEDANCE = 1, +// SIM_OBJECT_LCS_ROBOT_PLAN = 2 +// }; class C3 { public: @@ -35,9 +36,7 @@ class C3 { CostMatrices(const std::vector& Q, const std::vector& R, const std::vector& G, - const std::vector& U, - const std::vector& Q_evaluation = {}, - const std::vector& R_evaluation = {}); + const std::vector& U); bool HasSameDimensionsAs(const CostMatrices& other) const { // Check vector and matrix dimensions return (Q.size() == other.Q.size() && @@ -51,13 +50,7 @@ class C3 { G.at(0).cols() == other.G.at(0).cols() && U.size() == other.U.size() && U.at(0).rows() == other.U.at(0).rows() && - U.at(0).cols() == other.U.at(0).cols() && - Q_evaluation.size() == other.Q_evaluation.size() && - Q_evaluation.at(0).rows() == other.Q_evaluation.at(0).rows() && - Q_evaluation.at(0).cols() == other.Q_evaluation.at(0).cols() && - R_evaluation.size() == other.R_evaluation.size() && - R_evaluation.at(0).rows() == other.R_evaluation.at(0).rows() && - R_evaluation.at(0).cols() == other.R_evaluation.at(0).cols()); + U.at(0).cols() == other.U.at(0).cols()); } std::vector Q; std::vector R; @@ -205,6 +198,7 @@ class C3 { } /** + * TODO @bibit: remove * @brief Update the cost LCS used for cost evaluation in CalculateCost. * * @param cost_lcs the new LCS to be used for cost evaluation. Must have the @@ -215,9 +209,10 @@ class C3 { * planning LCS. Can differ in lambda dimensions, if higher contact resolution * is desired for evaluation. */ - void UpdateCostLCS(const LCS& cost_lcs); + // void UpdateCostLCS(const LCS& cost_lcs); /** + * TODO @bibit: remove * @brief Calculate the cost of the current solution, using the provided cost * type and optionally provided Kp and Kd values for the cost calculation. * @@ -233,9 +228,9 @@ class C3 { * @return A pair consisting of the cost (as a double) and a vector of the * state trajectory associated with the cost. */ - std::pair> CalculateCost( - const CostComputationType& cost_type, const Eigen::VectorXd& Kp = {}, - const Eigen::VectorXd& Kd = {}); + // std::pair> CalculateCost( + // const CostComputationType& cost_type, const Eigen::VectorXd& Kp = {}, + // const Eigen::VectorXd& Kd = {}); std::vector GetFullSolution() { return *z_sol_; } std::vector GetStateSolution() { return *x_sol_; } @@ -436,7 +431,7 @@ class C3 { int admm_iteration, bool is_final_solve); LCS lcs_; - LCS cost_lcs_; + // LCS cost_lcs_; // TODO @bibit: remove double AnDn_ = 1.0; // Scaling factor for lambdas CostMatrices cost_matrices_; std::vector x_desired_; diff --git a/core/lcs.cc b/core/lcs.cc index 3756c0b0..fcdc673c 100644 --- a/core/lcs.cc +++ b/core/lcs.cc @@ -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; diff --git a/core/lcs.h b/core/lcs.h index 8de9c14b..01e2a02b 100644 --- a/core/lcs.h +++ b/core/lcs.h @@ -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; /*! diff --git a/core/test/core_test.cc b/core/test/core_test.cc index 18815c4c..71f55f66 100644 --- a/core/test/core_test.cc +++ b/core/test/core_test.cc @@ -43,6 +43,7 @@ using namespace c3; * | CreatePlaceholderLCS | DONE | * | WarmStartSmokeTest | DONE | * | # of regression tests | 2 | + * | trajectory_evaluation | @bibit | * * It also has an E2E test for ensuring the "Solve()" function and other * internal functions are working as expected. However, the E2E takes about 120s diff --git a/core/trajectory_evaluation.cc b/core/trajectory_evaluation.cc new file mode 100644 index 00000000..b33977a5 --- /dev/null +++ b/core/trajectory_evaluation.cc @@ -0,0 +1,310 @@ +#include "trajectory_evaluation.h" + +#include +#include + +#include + +#include "lcs.h" +#include "solver_options_io.h" + +namespace c3 { + +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::vector; + +double ComputeQuadraticTrajectoryCost( + const vector& x, const vector& u, + const vector& lambda, const vector& x_des, + const vector& u_des, const vector& lambda_des, + const vector& Q, const vector& R, + const vector& S) { + DRAKE_DEMAND(x.size() == x_des.size() && x.size() == Q.size()); + DRAKE_DEMAND(u.size() == u_des.size() && u.size() == R.size()); + DRAKE_DEMAND(lambda.size() == lambda_des.size() && lambda.size() == S.size()); + DRAKE_DEMAND(x.size() - 1 == u.size() && u.size() == lambda.size()); + + int N = x.size() - 1; + int n_x = x[0].size(); + int n_u = u[0].size(); + int n_lambda = lambda[0].size(); + + double cost = 0.0; + + for (int i = 0; i < N + 1; i++) { + DRAKE_DEMAND(x[i].size() == n_x && x_des[i].size() == n_x); + DRAKE_DEMAND(Q[i].rows() == n_x && Q[i].cols() == n_x); + cost += (x[i] - x_des[i]).transpose() * Q[i] * (x[i] - x_des[i]); + + if (i < N) { + DRAKE_DEMAND(u[i].size() == n_u && u_des[i].size() == n_u); + DRAKE_DEMAND(R[i].rows() == n_u && R[i].cols() == n_u); + cost += (u[i] - u_des[i]).transpose() * R[i] * (u[i] - u_des[i]); + + DRAKE_DEMAND(lambda[i].size() == n_lambda && + lambda_des[i].size() == n_lambda); + DRAKE_DEMAND(S[i].rows() == n_lambda && S[i].cols() == n_lambda); + cost += (lambda[i] - lambda_des[i]).transpose() * S[i] * + (lambda[i] - lambda_des[i]); + } + } + + return cost; +} + +double ComputeQuadraticTrajectoryCost(const vector& x, + const vector& u, + const vector& x_des, + const vector& u_des, + const vector& Q, + const vector& R) { + return ComputeQuadraticTrajectoryCost( + x, u, vector(u.size(), VectorXd::Zero(0)), x_des, u_des, + vector(u.size(), VectorXd::Zero(0)), Q, R, + vector(u.size(), MatrixXd::Zero(0, 0))); +} + +double ComputeQuadraticTrajectoryCost(const vector& x, + const vector& x_des, + const vector& Q) { + return ComputeQuadraticTrajectoryCost( + x, vector(x.size() - 1, VectorXd::Zero(0)), + vector(x.size() - 1, VectorXd::Zero(0)), x_des, + vector(x.size() - 1, VectorXd::Zero(0)), + vector(x.size() - 1, VectorXd::Zero(0)), Q, + vector(x.size() - 1, MatrixXd::Zero(0, 0)), + vector(x.size() - 1, MatrixXd::Zero(0, 0))); +} + +double ComputeQuadraticTrajectoryCost( + const vector& x, const vector& u, + const vector& lambda, const vector& x_des, + const vector& u_des, const vector& lambda_des, + const MatrixXd& Q, const MatrixXd& R, const MatrixXd& S) { + return ComputeQuadraticTrajectoryCost( + x, u, lambda, x_des, u_des, lambda_des, vector(x.size(), Q), + vector(u.size(), R), vector(lambda.size(), S)); +} + +double ComputeQuadraticTrajectoryCost(const vector& x, + const vector& u, + const vector& x_des, + const vector& u_des, + const MatrixXd& Q, const MatrixXd& R) { + return ComputeQuadraticTrajectoryCost(x, u, x_des, u_des, + vector(x.size(), Q), + vector(u.size(), R)); +} + +double ComputeQuadraticTrajectoryCost(const vector& x, + const vector& x_des, + const MatrixXd& Q) { + return ComputeQuadraticTrajectoryCost(x, x_des, + vector(x.size(), Q)); +} + +std::pair, vector> SimulatePDControlWithLCS( + const vector& x_plan, const vector& u_plan, + const VectorXd& Kp, const VectorXd& Kd, const LCS& lcs) { + CheckLCSAndTrajectoryCompatibility(lcs, x_plan, u_plan); + + int N = lcs.N(); + int n_x = lcs.num_states(); + int n_u = lcs.num_inputs(); + int n_lambda = lcs.num_lambdas(); + + // Compute the new trajectory used for cost evaluation. + vector x(N + 1, VectorXd::Zero(n_x)); + vector u(N, VectorXd::Zero(n_u)); + + // Ensure the Kp and Kd vectors encode the actuated position and velocity + // indices within the state vector. + DRAKE_DEMAND(Kp.rows() == Kd.rows() && Kp.rows() == n_x); + DRAKE_DEMAND((Kp.array() != 0.0).count() == n_u); + DRAKE_DEMAND((Kd.array() != 0.0).count() == n_u); + MatrixXd Kp_mat = MatrixXd::Zero(n_u, n_x); + MatrixXd Kd_mat = MatrixXd::Zero(n_u, n_x); + int kp_i = 0; + int kd_i = 0; + for (int i = 0; i < n_x; ++i) { + if (Kp(i) != 0) { + Kp_mat(kp_i, i) = Kp(i); + kp_i++; + } + if (Kd(i) != 0) { + Kd_mat(kd_i, i) = Kd(i); + kd_i++; + } + } + + x[0] = x_plan[0]; + for (int i = 0; i < N; i++) { + u[i] = + u_plan[i] + Kp_mat * (x_plan[i] - x[i]) + Kd_mat * (x_plan[i] - x[i]); + x[i + 1] = lcs.Simulate(x[i], u[i]); + } + return std::make_pair(x, u); +} + +std::pair, vector> SimulatePDControlWithLCS( + const vector& x_plan, const vector& u_plan, + const VectorXd& Kp, const VectorXd& Kd, const LCS& coarse_lcs, + const LCS& fine_lcs) { + int timestep_split = CheckCoarseAndFineLCSCompatibility(coarse_lcs, fine_lcs); + + // Zero-order hold the planned trajectory to match the finer time + // discretization of the LCS. + std::pair, vector> fine_plans = + ZeroOrderHoldTrajectories(x_plan, u_plan, timestep_split); + vector x_plan_finer = fine_plans.first; + vector u_plan_finer = fine_plans.second; + + // Do PD control with the finer trajectory and LCS. + std::pair, vector> fine_sims = + SimulatePDControlWithLCS(x_plan_finer, u_plan_finer, Kp, Kd, fine_lcs); + vector x_sim_fine = fine_sims.first; + vector u_sim_fine = fine_sims.second; + + // Downsample the resulting trajectory back to the original time + // discretization. + std::pair, vector> downsampled_sims = + DownsampleTrajectories(x_sim_fine, u_sim_fine, timestep_split); + vector x_sim = downsampled_sims.first; + vector u_sim = downsampled_sims.second; + + return std::make_pair(x_sim, u_sim); +} + +vector SimulateLCSOverTrajectory(const VectorXd& x_init, + const vector& u_plan, + const LCS& lcs) { + int N = lcs.N(); + CheckLCSAndTrajectoryCompatibility(lcs, vector(N + 1, x_init), + u_plan); + + vector x_sim = + vector(N + 1, VectorXd::Zero(x_init.size())); + x_sim[0] = x_init; + for (int i = 0; i < N; i++) { + x_sim[i + 1] = lcs.Simulate(x_sim[i], u_plan[i]); + } + return x_sim; +} + +vector SimulateLCSOverTrajectory(const vector& x_plan, + const vector& u_plan, + const LCS& lcs) { + return SimulateLCSOverTrajectory(x_plan[0], u_plan, lcs); +} + +vector SimulateLCSOverTrajectory(const VectorXd& x_init, + const vector& u_plan, + const LCS& coarse_lcs, + const LCS& fine_lcs) { + int timestep_split = CheckCoarseAndFineLCSCompatibility(coarse_lcs, fine_lcs); + + vector u_plan_finer = + ZeroOrderHoldTrajectory(u_plan, timestep_split); + vector x_sim_fine = + SimulateLCSOverTrajectory(x_init, u_plan_finer, fine_lcs); + return DownsampleTrajectory(x_sim_fine, timestep_split); +} + +vector SimulateLCSOverTrajectory(const vector& x_plan, + const vector& u_plan, + const LCS& coarse_lcs, + const LCS& fine_lcs) { + return SimulateLCSOverTrajectory(x_plan[0], u_plan, coarse_lcs, fine_lcs); +} + +vector ZeroOrderHoldTrajectory(const vector& coarse_traj, + const int& timestep_split) { + int N = coarse_traj.size(); + + // Zero-order hold the planned trajectory to match the finer time + // discretization of the LCS. + vector fine_traj(N * timestep_split, + VectorXd::Zero(coarse_traj[0].size())); + for (int i = 0; i < N; i++) { + for (int j = 0; j < timestep_split; j++) { + fine_traj[i * timestep_split + j] = coarse_traj[i]; + } + } + return fine_traj; +} + +std::pair, vector> ZeroOrderHoldTrajectories( + const vector& x_coarse, const vector& u_coarse, + const int& timestep_split) { + DRAKE_DEMAND(x_coarse.size() == u_coarse.size() + 1); + vector x_fine = ZeroOrderHoldTrajectory(x_coarse, timestep_split); + vector u_fine = ZeroOrderHoldTrajectory(u_coarse, timestep_split); + return std::make_pair(x_fine, u_fine); +} + +vector DownsampleTrajectory(const vector& fine_traj, + const int& timestep_split) { + int N = fine_traj.size() / timestep_split; + + // Downsample the fine trajectory. + vector coarse_traj(N + 1, VectorXd::Zero(fine_traj[0].size())); + for (int i = 0; i < N; i++) { + coarse_traj[i] = fine_traj[i * timestep_split]; + } + return coarse_traj; +} + +std::pair, vector> DownsampleTrajectories( + const vector& x_fine, const vector& u_fine, + const int& timestep_split) { + DRAKE_DEMAND(x_fine.size() == u_fine.size() + 1); + vector x_coarse = DownsampleTrajectory(x_fine, timestep_split); + vector u_coarse = DownsampleTrajectory(u_fine, timestep_split); + return std::make_pair(x_coarse, u_coarse); +} + +int CheckCoarseAndFineLCSCompatibility(const LCS& coarse_lcs, + const LCS& fine_lcs) { + DRAKE_DEMAND(coarse_lcs.num_states() == fine_lcs.num_states()); + DRAKE_DEMAND(coarse_lcs.num_inputs() == fine_lcs.num_inputs()); + DRAKE_DEMAND(coarse_lcs.N() <= fine_lcs.N()); + DRAKE_DEMAND(coarse_lcs.dt() >= fine_lcs.dt()); + DRAKE_DEMAND(coarse_lcs.N() * coarse_lcs.dt() == + fine_lcs.N() * fine_lcs.dt()); + int timestep_split = fine_lcs.N() / coarse_lcs.N(); + DRAKE_DEMAND(fine_lcs.dt() * timestep_split == coarse_lcs.dt()); + DRAKE_DEMAND(coarse_lcs.N() * timestep_split == fine_lcs.N()); + return timestep_split; +} + +void CheckLCSAndTrajectoryCompatibility( + const LCS& lcs, const std::vector& x, + const std::vector& u, + const std::vector& lambda) { + DRAKE_DEMAND(lcs.N() == x.size() - 1 && lcs.N() == u.size() && + lcs.N() == lambda.size()); + for (int i = 0; i < lcs.N() + 1; i++) { + DRAKE_DEMAND(x[i].size() == lcs.num_states()); + if (i < lcs.N()) { + DRAKE_DEMAND(u[i].size() == lcs.num_inputs()); + DRAKE_DEMAND(lambda[i].size() == lcs.num_lambdas()); + } + } +} + +void CheckLCSAndTrajectoryCompatibility(const LCS& lcs, + const std::vector& x, + const std::vector& u) { + return CheckLCSAndTrajectoryCompatibility( + lcs, x, u, vector(lcs.N(), VectorXd::Zero(lcs.num_lambdas()))); +} + +void CheckLCSAndTrajectoryCompatibility(const LCS& lcs, + const std::vector& x) { + return CheckLCSAndTrajectoryCompatibility( + lcs, x, vector(lcs.N(), VectorXd::Zero(lcs.num_inputs())), + vector(lcs.N(), VectorXd::Zero(lcs.num_lambdas()))); +} + +} // namespace c3 diff --git a/core/trajectory_evaluation.h b/core/trajectory_evaluation.h new file mode 100644 index 00000000..c0138c71 --- /dev/null +++ b/core/trajectory_evaluation.h @@ -0,0 +1,160 @@ +#pragma once + +#include + +#include + +#include "c3_options.h" +#include "lcs.h" + +namespace c3 { + +enum CostComputationType : uint8_t { + SIM_IMPEDANCE = 1, + SIM_OBJECT_LCS_ROBOT_PLAN = 2 +}; + +double EvaluateTrajectoryCost(); + +/// Compute the quadratic cost of a trajectory of states, inputs, and contact +/// forces, with respect to a desired trajectory and quadratic cost matrices. +/// Cost = Sum_{i = 0, ... N-1} +/// (x_i-x_des_i)^T Q_i (x_i-x_des_i) + +/// (u_i-u_des_i)^T R_i (u_i-u_des_i) + +/// (lambda_i-lambda_des_i)^T S_i (lambda_i-lambda_des_i) +/// + (x_N-x_des_N)^T Q_N (x_N-x_des_N) +double ComputeQuadraticTrajectoryCost( + const std::vector& x, + const std::vector& u, + const std::vector& lambda, + const std::vector& x_des, + const std::vector& u_des, + const std::vector& lambda_des, + const std::vector& Q, + const std::vector& R, + const std::vector& S); +/// Special case: cost does not depend on the contact forces. +double ComputeQuadraticTrajectoryCost(const std::vector& x, + const std::vector& u, + const std::vector& x_des, + const std::vector& u_des, + const std::vector& Q, + const std::vector& R); +/// Special case: cost does not depend on the inputs or contact forces. +double ComputeQuadraticTrajectoryCost(const std::vector& x, + const std::vector& x_des, + const std::vector& Q); +/// Special case: cost matrices are the same across all time steps. +double ComputeQuadraticTrajectoryCost( + const std::vector& x, + const std::vector& u, + const std::vector& lambda, + const std::vector& x_des, + const std::vector& u_des, + const std::vector& lambda_des, const Eigen::MatrixXd& Q, + const Eigen::MatrixXd& R, const Eigen::MatrixXd& S); +/// Special case: cost does not depend on the contact forces, and cost matrices +/// are the same across all time steps. +double ComputeQuadraticTrajectoryCost(const std::vector& x, + const std::vector& u, + const std::vector& x_des, + const std::vector& u_des, + const Eigen::MatrixXd& Q, + const Eigen::MatrixXd& R); +/// Special case: cost does not depend on the inputs or contact forces, and cost +/// matrices are the same across all time steps. +double ComputeQuadraticTrajectoryCost(const std::vector& x, + const std::vector& x_des, + const Eigen::MatrixXd& Q); + +/// From a planned trajectory of states and inputs and sets of Kp and Kd gains, +/// use an LCS to simulate tracking the plan with PD control (with feedforward +/// control), and return the resulting trajectory of actual states and inputs. +std::pair, std::vector> +SimulatePDControlWithLCS(const std::vector& x_plan, + const std::vector& u_plan, + const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd, + const LCS& lcs); +/// Special case: simulate plans from a coarser LCS with a finer LCS. The +/// returned trajectory is downsampled back to be compatible with the coarser +/// LCS. The two LCSs must be compatible with each other, and the state and +/// input plans must be compatible with the coarser LCS. +std::pair, std::vector> +SimulatePDControlWithLCS(const std::vector& x_plan, + const std::vector& u_plan, + const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd, + const LCS& coarse_lcs, const LCS& fine_lcs); + +/// Simulate an LCS forward from an initial condition over a trajectory of +/// inputs, and return the resulting trajectory of states. +std::vector SimulateLCSOverTrajectory( + const Eigen::VectorXd& x_init, const std::vector& u_plan, + const LCS& lcs); +/// Special case: a full trajectory of states is provided, but only the initial +/// state is used for simulation. +std::vector SimulateLCSOverTrajectory( + const std::vector& x_plan, + const std::vector& u_plan, const LCS& lcs); +/// Special case: simulate a trajectory from a coarser LCS with a finer LCS. +std::vector SimulateLCSOverTrajectory( + const Eigen::VectorXd& x_init, const std::vector& u_plan, + const LCS& coarse_lcs, const LCS& fine_lcs); +/// Special case: simulate a trajectory from a coarser LCS with a finer LCS, +/// where a full trajectory of states is provided but only the initial state is +/// used. +std::vector SimulateLCSOverTrajectory( + const std::vector& x_plan, + const std::vector& u_plan, const LCS& coarse_lcs, + const LCS& fine_lcs); + +/// Zero order hold a trajectory compatible with one LCS to be compatible with +/// a different LCS with a finer time discretization. The LCSs must be +/// compatible with each other, i.e. the finer horizon must be an integer +/// multiple of the coarser horizon, and dt*N must be the same for both LCSs. +std::pair, std::vector> +ZeroOrderHoldTrajectories(const std::vector& x_coarse, + const std::vector& u_coarse, + const int& timestep_split); +/// Same as above but only one trajectory is provided and processed. +std::vector ZeroOrderHoldTrajectory( + const std::vector& coarse_traj, const int& timestep_split); + +/// The reverse of ZeroOrderHoldTrajectoryToFinerLCS: downsample a trajectory +/// compatible with a finer LCS to be compatible with a coarser LCS. The same +/// compatibility requirements apply as for ZeroOrderHoldTrajectoryToFinerLCS. +std::pair, std::vector> +DownsampleTrajectories(const std::vector& x_fine, + const std::vector& u_fine, + const int& timestep_split); +/// Same as above but only one trajectory is provided and processed. +std::vector DownsampleTrajectory( + const std::vector& fine_traj, const int& timestep_split); + +/// Check that two LCSs are compatible with each other, i.e. that the finer +/// horizon is an integer multiple of the coarser horizon, and that dt*N is the +/// same for both LCSs. Returns the integer multiple by which the finer horizon +/// is larger than the coarser horizon (i.e. the number of finer time steps per +/// coarser time step). +/// NOTE: This function does not assert that the LCSs' lambda dimensions are the +/// same, since it may be desirable to use different contact pairs for each LCS. +/// The state and input dimensions must be the same. +int CheckCoarseAndFineLCSCompatibility(const LCS& coarse_lcs, + const LCS& fine_lcs); + +/// Check that a trajectory of states, inputs, and contact forces is compatible +/// with an LCS, i.e. that the trajectory has the correct number of time steps +/// and that the dimensions of the states, inputs, and contact forces match the +/// dimensions of the LCS. +void CheckLCSAndTrajectoryCompatibility( + const LCS& lcs, const std::vector& x, + const std::vector& u, + const std::vector& lambda); +/// Special case: no lambdas provided. +void CheckLCSAndTrajectoryCompatibility(const LCS& lcs, + const std::vector& x, + const std::vector& u); +/// Special case: no inputs or lambdas provided. +void CheckLCSAndTrajectoryCompatibility(const LCS& lcs, + const std::vector& x); + +} // namespace c3 From df22ca5d19895f9725dd376c8dd04eb4c0304d15 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Thu, 26 Feb 2026 12:13:36 -0500 Subject: [PATCH 04/17] Restructure to use namespace and static class functions --- core/BUILD.bazel | 6 +- core/test/core_test.cc | 2 +- ...{trajectory_evaluation.cc => traj_eval.cc} | 122 +++++++------ core/traj_eval.h | 170 ++++++++++++++++++ core/trajectory_evaluation.h | 160 ----------------- 5 files changed, 237 insertions(+), 223 deletions(-) rename core/{trajectory_evaluation.cc => traj_eval.cc} (70%) create mode 100644 core/traj_eval.h delete mode 100644 core/trajectory_evaluation.h diff --git a/core/BUILD.bazel b/core/BUILD.bazel index a5a6ac55..c7620916 100644 --- a/core/BUILD.bazel +++ b/core/BUILD.bazel @@ -75,9 +75,9 @@ cc_library( ) cc_library( - name = "trajectory_evaluation", - srcs = ["trajectory_evaluation.cc"], - hdrs = ["trajectory_evaluation.h"], + name = "traj_eval", + srcs = ["traj_eval.cc"], + hdrs = ["traj_eval.h"], deps = [ ":c3", "@drake//:drake_shared_library", diff --git a/core/test/core_test.cc b/core/test/core_test.cc index 71f55f66..09e82b53 100644 --- a/core/test/core_test.cc +++ b/core/test/core_test.cc @@ -43,7 +43,7 @@ using namespace c3; * | CreatePlaceholderLCS | DONE | * | WarmStartSmokeTest | DONE | * | # of regression tests | 2 | - * | trajectory_evaluation | @bibit | + * | traj_eval | @bibit | * * It also has an E2E test for ensuring the "Solve()" function and other * internal functions are working as expected. However, the E2E takes about 120s diff --git a/core/trajectory_evaluation.cc b/core/traj_eval.cc similarity index 70% rename from core/trajectory_evaluation.cc rename to core/traj_eval.cc index b33977a5..8cec7304 100644 --- a/core/trajectory_evaluation.cc +++ b/core/traj_eval.cc @@ -1,4 +1,4 @@ -#include "trajectory_evaluation.h" +#include "traj_eval.h" #include #include @@ -9,12 +9,13 @@ #include "solver_options_io.h" namespace c3 { +namespace traj_eval { using Eigen::MatrixXd; using Eigen::VectorXd; using std::vector; -double ComputeQuadraticTrajectoryCost( +double TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( const vector& x, const vector& u, const vector& lambda, const vector& x_des, const vector& u_des, const vector& lambda_des, @@ -53,21 +54,19 @@ double ComputeQuadraticTrajectoryCost( return cost; } -double ComputeQuadraticTrajectoryCost(const vector& x, - const vector& u, - const vector& x_des, - const vector& u_des, - const vector& Q, - const vector& R) { +double TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + const vector& x, const vector& u, + const vector& x_des, const vector& u_des, + const vector& Q, const vector& R) { return ComputeQuadraticTrajectoryCost( x, u, vector(u.size(), VectorXd::Zero(0)), x_des, u_des, vector(u.size(), VectorXd::Zero(0)), Q, R, vector(u.size(), MatrixXd::Zero(0, 0))); } -double ComputeQuadraticTrajectoryCost(const vector& x, - const vector& x_des, - const vector& Q) { +double TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + const vector& x, const vector& x_des, + const vector& Q) { return ComputeQuadraticTrajectoryCost( x, vector(x.size() - 1, VectorXd::Zero(0)), vector(x.size() - 1, VectorXd::Zero(0)), x_des, @@ -77,7 +76,7 @@ double ComputeQuadraticTrajectoryCost(const vector& x, vector(x.size() - 1, MatrixXd::Zero(0, 0))); } -double ComputeQuadraticTrajectoryCost( +double TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( const vector& x, const vector& u, const vector& lambda, const vector& x_des, const vector& u_des, const vector& lambda_des, @@ -87,26 +86,28 @@ double ComputeQuadraticTrajectoryCost( vector(u.size(), R), vector(lambda.size(), S)); } -double ComputeQuadraticTrajectoryCost(const vector& x, - const vector& u, - const vector& x_des, - const vector& u_des, - const MatrixXd& Q, const MatrixXd& R) { +double TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + const vector& x, const vector& u, + const vector& x_des, const vector& u_des, + const MatrixXd& Q, const MatrixXd& R) { return ComputeQuadraticTrajectoryCost(x, u, x_des, u_des, vector(x.size(), Q), vector(u.size(), R)); } -double ComputeQuadraticTrajectoryCost(const vector& x, - const vector& x_des, - const MatrixXd& Q) { +double TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + const vector& x, const vector& x_des, + const MatrixXd& Q) { return ComputeQuadraticTrajectoryCost(x, x_des, vector(x.size(), Q)); } -std::pair, vector> SimulatePDControlWithLCS( - const vector& x_plan, const vector& u_plan, - const VectorXd& Kp, const VectorXd& Kd, const LCS& lcs) { +std::pair, vector> +TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, + const vector& u_plan, + const VectorXd& Kp, + const VectorXd& Kd, + const LCS& lcs) { CheckLCSAndTrajectoryCompatibility(lcs, x_plan, u_plan); int N = lcs.N(); @@ -147,10 +148,13 @@ std::pair, vector> SimulatePDControlWithLCS( return std::make_pair(x, u); } -std::pair, vector> SimulatePDControlWithLCS( - const vector& x_plan, const vector& u_plan, - const VectorXd& Kp, const VectorXd& Kd, const LCS& coarse_lcs, - const LCS& fine_lcs) { +std::pair, vector> +TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, + const vector& u_plan, + const VectorXd& Kp, + const VectorXd& Kd, + const LCS& coarse_lcs, + const LCS& fine_lcs) { int timestep_split = CheckCoarseAndFineLCSCompatibility(coarse_lcs, fine_lcs); // Zero-order hold the planned trajectory to match the finer time @@ -176,9 +180,8 @@ std::pair, vector> SimulatePDControlWithLCS( return std::make_pair(x_sim, u_sim); } -vector SimulateLCSOverTrajectory(const VectorXd& x_init, - const vector& u_plan, - const LCS& lcs) { +vector TrajectoryEvaluator::SimulateLCSOverTrajectory( + const VectorXd& x_init, const vector& u_plan, const LCS& lcs) { int N = lcs.N(); CheckLCSAndTrajectoryCompatibility(lcs, vector(N + 1, x_init), u_plan); @@ -192,16 +195,15 @@ vector SimulateLCSOverTrajectory(const VectorXd& x_init, return x_sim; } -vector SimulateLCSOverTrajectory(const vector& x_plan, - const vector& u_plan, - const LCS& lcs) { +vector TrajectoryEvaluator::SimulateLCSOverTrajectory( + const vector& x_plan, const vector& u_plan, + const LCS& lcs) { return SimulateLCSOverTrajectory(x_plan[0], u_plan, lcs); } -vector SimulateLCSOverTrajectory(const VectorXd& x_init, - const vector& u_plan, - const LCS& coarse_lcs, - const LCS& fine_lcs) { +vector TrajectoryEvaluator::SimulateLCSOverTrajectory( + const VectorXd& x_init, const vector& u_plan, + const LCS& coarse_lcs, const LCS& fine_lcs) { int timestep_split = CheckCoarseAndFineLCSCompatibility(coarse_lcs, fine_lcs); vector u_plan_finer = @@ -211,15 +213,14 @@ vector SimulateLCSOverTrajectory(const VectorXd& x_init, return DownsampleTrajectory(x_sim_fine, timestep_split); } -vector SimulateLCSOverTrajectory(const vector& x_plan, - const vector& u_plan, - const LCS& coarse_lcs, - const LCS& fine_lcs) { +vector TrajectoryEvaluator::SimulateLCSOverTrajectory( + const vector& x_plan, const vector& u_plan, + const LCS& coarse_lcs, const LCS& fine_lcs) { return SimulateLCSOverTrajectory(x_plan[0], u_plan, coarse_lcs, fine_lcs); } -vector ZeroOrderHoldTrajectory(const vector& coarse_traj, - const int& timestep_split) { +vector TrajectoryEvaluator::ZeroOrderHoldTrajectory( + const vector& coarse_traj, const int& timestep_split) { int N = coarse_traj.size(); // Zero-order hold the planned trajectory to match the finer time @@ -234,17 +235,18 @@ vector ZeroOrderHoldTrajectory(const vector& coarse_traj, return fine_traj; } -std::pair, vector> ZeroOrderHoldTrajectories( - const vector& x_coarse, const vector& u_coarse, - const int& timestep_split) { +std::pair, vector> +TrajectoryEvaluator::ZeroOrderHoldTrajectories(const vector& x_coarse, + const vector& u_coarse, + const int& timestep_split) { DRAKE_DEMAND(x_coarse.size() == u_coarse.size() + 1); vector x_fine = ZeroOrderHoldTrajectory(x_coarse, timestep_split); vector u_fine = ZeroOrderHoldTrajectory(u_coarse, timestep_split); return std::make_pair(x_fine, u_fine); } -vector DownsampleTrajectory(const vector& fine_traj, - const int& timestep_split) { +vector TrajectoryEvaluator::DownsampleTrajectory( + const vector& fine_traj, const int& timestep_split) { int N = fine_traj.size() / timestep_split; // Downsample the fine trajectory. @@ -255,17 +257,18 @@ vector DownsampleTrajectory(const vector& fine_traj, return coarse_traj; } -std::pair, vector> DownsampleTrajectories( - const vector& x_fine, const vector& u_fine, - const int& timestep_split) { +std::pair, vector> +TrajectoryEvaluator::DownsampleTrajectories(const vector& x_fine, + const vector& u_fine, + const int& timestep_split) { DRAKE_DEMAND(x_fine.size() == u_fine.size() + 1); vector x_coarse = DownsampleTrajectory(x_fine, timestep_split); vector u_coarse = DownsampleTrajectory(u_fine, timestep_split); return std::make_pair(x_coarse, u_coarse); } -int CheckCoarseAndFineLCSCompatibility(const LCS& coarse_lcs, - const LCS& fine_lcs) { +int TrajectoryEvaluator::CheckCoarseAndFineLCSCompatibility( + const LCS& coarse_lcs, const LCS& fine_lcs) { DRAKE_DEMAND(coarse_lcs.num_states() == fine_lcs.num_states()); DRAKE_DEMAND(coarse_lcs.num_inputs() == fine_lcs.num_inputs()); DRAKE_DEMAND(coarse_lcs.N() <= fine_lcs.N()); @@ -278,7 +281,7 @@ int CheckCoarseAndFineLCSCompatibility(const LCS& coarse_lcs, return timestep_split; } -void CheckLCSAndTrajectoryCompatibility( +void TrajectoryEvaluator::CheckLCSAndTrajectoryCompatibility( const LCS& lcs, const std::vector& x, const std::vector& u, const std::vector& lambda) { @@ -293,18 +296,19 @@ void CheckLCSAndTrajectoryCompatibility( } } -void CheckLCSAndTrajectoryCompatibility(const LCS& lcs, - const std::vector& x, - const std::vector& u) { +void TrajectoryEvaluator::CheckLCSAndTrajectoryCompatibility( + const LCS& lcs, const std::vector& x, + const std::vector& u) { return CheckLCSAndTrajectoryCompatibility( lcs, x, u, vector(lcs.N(), VectorXd::Zero(lcs.num_lambdas()))); } -void CheckLCSAndTrajectoryCompatibility(const LCS& lcs, - const std::vector& x) { +void TrajectoryEvaluator::CheckLCSAndTrajectoryCompatibility( + const LCS& lcs, const std::vector& x) { return CheckLCSAndTrajectoryCompatibility( lcs, x, vector(lcs.N(), VectorXd::Zero(lcs.num_inputs())), vector(lcs.N(), VectorXd::Zero(lcs.num_lambdas()))); } +} // namespace traj_eval } // namespace c3 diff --git a/core/traj_eval.h b/core/traj_eval.h new file mode 100644 index 00000000..d9f093b5 --- /dev/null +++ b/core/traj_eval.h @@ -0,0 +1,170 @@ +#pragma once + +#include + +#include + +#include "c3_options.h" +#include "lcs.h" + +namespace c3 { +namespace traj_eval { + +/// Utility class for trajectory evaluation computations and simulations. +class TrajectoryEvaluator { + public: + enum CostComputationType : uint8_t { + SIM_IMPEDANCE = 1, + SIM_OBJECT_LCS_ROBOT_PLAN = 2 + }; + + static double EvaluateTrajectoryCost(); + + /// Compute the quadratic cost of a trajectory of states, inputs, and contact + /// forces, with respect to a desired trajectory and quadratic cost matrices. + /// Cost = Sum_{i = 0, ... N-1} + /// (x_i-x_des_i)^T Q_i (x_i-x_des_i) + + /// (u_i-u_des_i)^T R_i (u_i-u_des_i) + + /// (lambda_i-lambda_des_i)^T S_i (lambda_i-lambda_des_i) + /// + (x_N-x_des_N)^T Q_N (x_N-x_des_N) + static double ComputeQuadraticTrajectoryCost( + const std::vector& x, + const std::vector& u, + const std::vector& lambda, + const std::vector& x_des, + const std::vector& u_des, + const std::vector& lambda_des, + const std::vector& Q, + const std::vector& R, + const std::vector& S); + /// Special case: cost does not depend on the contact forces. + static double ComputeQuadraticTrajectoryCost( + const std::vector& x, + const std::vector& u, + const std::vector& x_des, + const std::vector& u_des, + const std::vector& Q, + const std::vector& R); + /// Special case: cost does not depend on the inputs or contact forces. + static double ComputeQuadraticTrajectoryCost( + const std::vector& x, + const std::vector& x_des, + const std::vector& Q); + /// Special case: cost matrices are the same across all time steps. + static double ComputeQuadraticTrajectoryCost( + const std::vector& x, + const std::vector& u, + const std::vector& lambda, + const std::vector& x_des, + const std::vector& u_des, + const std::vector& lambda_des, const Eigen::MatrixXd& Q, + const Eigen::MatrixXd& R, const Eigen::MatrixXd& S); + /// Special case: cost does not depend on the contact forces, and cost + /// matrices are the same across all time steps. + static double ComputeQuadraticTrajectoryCost( + const std::vector& x, + const std::vector& u, + const std::vector& x_des, + const std::vector& u_des, const Eigen::MatrixXd& Q, + const Eigen::MatrixXd& R); + /// Special case: cost does not depend on the inputs or contact forces, and + /// cost matrices are the same across all time steps. + static double ComputeQuadraticTrajectoryCost( + const std::vector& x, + const std::vector& x_des, const Eigen::MatrixXd& Q); + + /// From a planned trajectory of states and inputs and sets of Kp and Kd + /// gains, use an LCS to simulate tracking the plan with PD control (with + /// feedforward control), and return the resulting trajectory of actual states + /// and inputs. + static std::pair, std::vector> + SimulatePDControlWithLCS(const std::vector& x_plan, + const std::vector& u_plan, + const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd, + const LCS& lcs); + /// Special case: simulate plans from a coarser LCS with a finer LCS. The + /// returned trajectory is downsampled back to be compatible with the coarser + /// LCS. The two LCSs must be compatible with each other, and the state and + /// input plans must be compatible with the coarser LCS. + static std::pair, std::vector> + SimulatePDControlWithLCS(const std::vector& x_plan, + const std::vector& u_plan, + const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd, + const LCS& coarse_lcs, const LCS& fine_lcs); + + /// Simulate an LCS forward from an initial condition over a trajectory of + /// inputs, and return the resulting trajectory of states. + static std::vector SimulateLCSOverTrajectory( + const Eigen::VectorXd& x_init, const std::vector& u_plan, + const LCS& lcs); + /// Special case: a full trajectory of states is provided, but only the + /// initial state is used for simulation. + static std::vector SimulateLCSOverTrajectory( + const std::vector& x_plan, + const std::vector& u_plan, const LCS& lcs); + /// Special case: simulate a trajectory from a coarser LCS with a finer LCS. + static std::vector SimulateLCSOverTrajectory( + const Eigen::VectorXd& x_init, const std::vector& u_plan, + const LCS& coarse_lcs, const LCS& fine_lcs); + /// Special case: simulate a trajectory from a coarser LCS with a finer LCS, + /// where a full trajectory of states is provided but only the initial state + /// is used. + static std::vector SimulateLCSOverTrajectory( + const std::vector& x_plan, + const std::vector& u_plan, const LCS& coarse_lcs, + const LCS& fine_lcs); + + /// Zero order hold a trajectory compatible with one LCS to be compatible with + /// a different LCS with a finer time discretization. The LCSs must be + /// compatible with each other, i.e. the finer horizon must be an integer + /// multiple of the coarser horizon, and dt*N must be the same for both LCSs. + static std::pair, std::vector> + ZeroOrderHoldTrajectories(const std::vector& x_coarse, + const std::vector& u_coarse, + const int& timestep_split); + /// Same as above but only one trajectory is provided and processed. + static std::vector ZeroOrderHoldTrajectory( + const std::vector& coarse_traj, + const int& timestep_split); + + /// The reverse of ZeroOrderHoldTrajectoryToFinerLCS: downsample a trajectory + /// compatible with a finer LCS to be compatible with a coarser LCS. The same + /// compatibility requirements apply as for ZeroOrderHoldTrajectoryToFinerLCS. + static std::pair, std::vector> + DownsampleTrajectories(const std::vector& x_fine, + const std::vector& u_fine, + const int& timestep_split); + /// Same as above but only one trajectory is provided and processed. + static std::vector DownsampleTrajectory( + const std::vector& fine_traj, const int& timestep_split); + + /// Check that two LCSs are compatible with each other, i.e. that the finer + /// horizon is an integer multiple of the coarser horizon, and that dt*N is + /// the same for both LCSs. Returns the integer multiple by which the finer + /// horizon is larger than the coarser horizon (i.e. the number of finer time + /// steps per coarser time step). NOTE: This function does not assert that the + /// LCSs' lambda dimensions are the same, since it may be desirable to use + /// different contact pairs for each LCS. The state and input dimensions must + /// be the same. + static int CheckCoarseAndFineLCSCompatibility(const LCS& coarse_lcs, + const LCS& fine_lcs); + + /// Check that a trajectory of states, inputs, and contact forces is + /// compatible with an LCS, i.e. that the trajectory has the correct number of + /// time steps and that the dimensions of the states, inputs, and contact + /// forces match the dimensions of the LCS. + static void CheckLCSAndTrajectoryCompatibility( + const LCS& lcs, const std::vector& x, + const std::vector& u, + const std::vector& lambda); + /// Special case: no lambdas provided. + static void CheckLCSAndTrajectoryCompatibility( + const LCS& lcs, const std::vector& x, + const std::vector& u); + /// Special case: no inputs or lambdas provided. + static void CheckLCSAndTrajectoryCompatibility( + const LCS& lcs, const std::vector& x); +}; + +} // namespace traj_eval +} // namespace c3 diff --git a/core/trajectory_evaluation.h b/core/trajectory_evaluation.h deleted file mode 100644 index c0138c71..00000000 --- a/core/trajectory_evaluation.h +++ /dev/null @@ -1,160 +0,0 @@ -#pragma once - -#include - -#include - -#include "c3_options.h" -#include "lcs.h" - -namespace c3 { - -enum CostComputationType : uint8_t { - SIM_IMPEDANCE = 1, - SIM_OBJECT_LCS_ROBOT_PLAN = 2 -}; - -double EvaluateTrajectoryCost(); - -/// Compute the quadratic cost of a trajectory of states, inputs, and contact -/// forces, with respect to a desired trajectory and quadratic cost matrices. -/// Cost = Sum_{i = 0, ... N-1} -/// (x_i-x_des_i)^T Q_i (x_i-x_des_i) + -/// (u_i-u_des_i)^T R_i (u_i-u_des_i) + -/// (lambda_i-lambda_des_i)^T S_i (lambda_i-lambda_des_i) -/// + (x_N-x_des_N)^T Q_N (x_N-x_des_N) -double ComputeQuadraticTrajectoryCost( - const std::vector& x, - const std::vector& u, - const std::vector& lambda, - const std::vector& x_des, - const std::vector& u_des, - const std::vector& lambda_des, - const std::vector& Q, - const std::vector& R, - const std::vector& S); -/// Special case: cost does not depend on the contact forces. -double ComputeQuadraticTrajectoryCost(const std::vector& x, - const std::vector& u, - const std::vector& x_des, - const std::vector& u_des, - const std::vector& Q, - const std::vector& R); -/// Special case: cost does not depend on the inputs or contact forces. -double ComputeQuadraticTrajectoryCost(const std::vector& x, - const std::vector& x_des, - const std::vector& Q); -/// Special case: cost matrices are the same across all time steps. -double ComputeQuadraticTrajectoryCost( - const std::vector& x, - const std::vector& u, - const std::vector& lambda, - const std::vector& x_des, - const std::vector& u_des, - const std::vector& lambda_des, const Eigen::MatrixXd& Q, - const Eigen::MatrixXd& R, const Eigen::MatrixXd& S); -/// Special case: cost does not depend on the contact forces, and cost matrices -/// are the same across all time steps. -double ComputeQuadraticTrajectoryCost(const std::vector& x, - const std::vector& u, - const std::vector& x_des, - const std::vector& u_des, - const Eigen::MatrixXd& Q, - const Eigen::MatrixXd& R); -/// Special case: cost does not depend on the inputs or contact forces, and cost -/// matrices are the same across all time steps. -double ComputeQuadraticTrajectoryCost(const std::vector& x, - const std::vector& x_des, - const Eigen::MatrixXd& Q); - -/// From a planned trajectory of states and inputs and sets of Kp and Kd gains, -/// use an LCS to simulate tracking the plan with PD control (with feedforward -/// control), and return the resulting trajectory of actual states and inputs. -std::pair, std::vector> -SimulatePDControlWithLCS(const std::vector& x_plan, - const std::vector& u_plan, - const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd, - const LCS& lcs); -/// Special case: simulate plans from a coarser LCS with a finer LCS. The -/// returned trajectory is downsampled back to be compatible with the coarser -/// LCS. The two LCSs must be compatible with each other, and the state and -/// input plans must be compatible with the coarser LCS. -std::pair, std::vector> -SimulatePDControlWithLCS(const std::vector& x_plan, - const std::vector& u_plan, - const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd, - const LCS& coarse_lcs, const LCS& fine_lcs); - -/// Simulate an LCS forward from an initial condition over a trajectory of -/// inputs, and return the resulting trajectory of states. -std::vector SimulateLCSOverTrajectory( - const Eigen::VectorXd& x_init, const std::vector& u_plan, - const LCS& lcs); -/// Special case: a full trajectory of states is provided, but only the initial -/// state is used for simulation. -std::vector SimulateLCSOverTrajectory( - const std::vector& x_plan, - const std::vector& u_plan, const LCS& lcs); -/// Special case: simulate a trajectory from a coarser LCS with a finer LCS. -std::vector SimulateLCSOverTrajectory( - const Eigen::VectorXd& x_init, const std::vector& u_plan, - const LCS& coarse_lcs, const LCS& fine_lcs); -/// Special case: simulate a trajectory from a coarser LCS with a finer LCS, -/// where a full trajectory of states is provided but only the initial state is -/// used. -std::vector SimulateLCSOverTrajectory( - const std::vector& x_plan, - const std::vector& u_plan, const LCS& coarse_lcs, - const LCS& fine_lcs); - -/// Zero order hold a trajectory compatible with one LCS to be compatible with -/// a different LCS with a finer time discretization. The LCSs must be -/// compatible with each other, i.e. the finer horizon must be an integer -/// multiple of the coarser horizon, and dt*N must be the same for both LCSs. -std::pair, std::vector> -ZeroOrderHoldTrajectories(const std::vector& x_coarse, - const std::vector& u_coarse, - const int& timestep_split); -/// Same as above but only one trajectory is provided and processed. -std::vector ZeroOrderHoldTrajectory( - const std::vector& coarse_traj, const int& timestep_split); - -/// The reverse of ZeroOrderHoldTrajectoryToFinerLCS: downsample a trajectory -/// compatible with a finer LCS to be compatible with a coarser LCS. The same -/// compatibility requirements apply as for ZeroOrderHoldTrajectoryToFinerLCS. -std::pair, std::vector> -DownsampleTrajectories(const std::vector& x_fine, - const std::vector& u_fine, - const int& timestep_split); -/// Same as above but only one trajectory is provided and processed. -std::vector DownsampleTrajectory( - const std::vector& fine_traj, const int& timestep_split); - -/// Check that two LCSs are compatible with each other, i.e. that the finer -/// horizon is an integer multiple of the coarser horizon, and that dt*N is the -/// same for both LCSs. Returns the integer multiple by which the finer horizon -/// is larger than the coarser horizon (i.e. the number of finer time steps per -/// coarser time step). -/// NOTE: This function does not assert that the LCSs' lambda dimensions are the -/// same, since it may be desirable to use different contact pairs for each LCS. -/// The state and input dimensions must be the same. -int CheckCoarseAndFineLCSCompatibility(const LCS& coarse_lcs, - const LCS& fine_lcs); - -/// Check that a trajectory of states, inputs, and contact forces is compatible -/// with an LCS, i.e. that the trajectory has the correct number of time steps -/// and that the dimensions of the states, inputs, and contact forces match the -/// dimensions of the LCS. -void CheckLCSAndTrajectoryCompatibility( - const LCS& lcs, const std::vector& x, - const std::vector& u, - const std::vector& lambda); -/// Special case: no lambdas provided. -void CheckLCSAndTrajectoryCompatibility(const LCS& lcs, - const std::vector& x, - const std::vector& u); -/// Special case: no inputs or lambdas provided. -void CheckLCSAndTrajectoryCompatibility(const LCS& lcs, - const std::vector& x); - -} // namespace c3 From 16bc9450addf2917eb5a7770c91d5c70d9bb75e2 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Mon, 2 Mar 2026 14:09:35 -0500 Subject: [PATCH 05/17] Add unit tests for trajectory evaluation functions --- core/BUILD.bazel | 1 + core/test/core_test.cc | 302 +++++++++++++++++++++++++++++++++++++++++ core/traj_eval.cc | 72 +++++----- core/traj_eval.h | 11 +- 4 files changed, 346 insertions(+), 40 deletions(-) diff --git a/core/BUILD.bazel b/core/BUILD.bazel index c7620916..ebca23fe 100644 --- a/core/BUILD.bazel +++ b/core/BUILD.bazel @@ -108,6 +108,7 @@ cc_test( deps = [ ":c3", ":c3_cartpole_problem", + ":traj_eval", "@gtest//:main", ], env_inherit = [ diff --git a/core/test/core_test.cc b/core/test/core_test.cc index 09e82b53..22d01452 100644 --- a/core/test/core_test.cc +++ b/core/test/core_test.cc @@ -9,6 +9,7 @@ #include "core/c3_qp.h" #include "core/lcs.h" #include "core/test/c3_cartpole_problem.hpp" +#include "core/traj_eval.h" #include "drake/math/discrete_algebraic_riccati_equation.h" @@ -18,6 +19,7 @@ using Eigen::VectorXd; using std::vector; using c3::C3Options; +using c3::traj_eval::TrajectoryEvaluator; using namespace c3; @@ -321,6 +323,306 @@ TEST_F(C3CartpoleTest, WarmStartSmokeTest) { ASSERT_NO_THROW(optimizer.Solve(x0)); } +class TrajectoryEvaluatorTest : public testing::Test {}; + +TEST_F(TrajectoryEvaluatorTest, QuadraticCostMatchesManual) { + std::vector x(2, VectorXd::Zero(1)); + std::vector u(1, VectorXd::Zero(1)); + std::vector lambda(1, VectorXd::Zero(1)); + std::vector x_des(2, VectorXd::Zero(1)); + std::vector u_des(1, VectorXd::Zero(1)); + std::vector lambda_des(1, VectorXd::Zero(1)); + + x[0](0) = 1.0; + x[1](0) = 2.0; + u[0](0) = 3.0; + lambda[0](0) = 0.25; + x_des[0](0) = 0.5; + x_des[1](0) = 1.5; + u_des[0](0) = 2.0; + lambda_des[0](0) = 0.0; + + std::vector Q(2, MatrixXd::Zero(1, 1)); + std::vector R(1, MatrixXd::Zero(1, 1)); + std::vector S(1, MatrixXd::Zero(1, 1)); + Q[0](0, 0) = 2.0; + Q[1](0, 0) = 4.0; + R[0](0, 0) = 3.0; + S[0](0, 0) = 5.0; + + double expected = 0.0; + expected += 2.0 * 0.5 * 0.5; + expected += 4.0 * 0.5 * 0.5; + double actual = + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(x, x_des, Q); + EXPECT_NEAR(actual, expected, 1e-12); // State costs + + expected += 3.0 * 1.0 * 1.0; + actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(x, u, x_des, + u_des, Q, R); + EXPECT_NEAR(actual, expected, 1e-12); // State and input costs + + expected += 5.0 * 0.25 * 0.25; + actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + x, u, lambda, x_des, u_des, lambda_des, Q, R, S); + EXPECT_NEAR(actual, expected, 1e-12); // State, input, force costs + + // Test with repeated cost matrices across time steps + expected = 0.0; + expected += 2.0 * 0.5 * 0.5; + expected += 2.0 * 0.5 * 0.5; + actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(x, x_des, Q[0]); + EXPECT_NEAR(actual, expected, 1e-12); // State costs + + expected += 3.0 * 1.0 * 1.0; + actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + x, u, x_des, u_des, Q[0], R[0]); + EXPECT_NEAR(actual, expected, 1e-12); // State and input costs + + expected += 5.0 * 0.25 * 0.25; + actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + x, u, lambda, x_des, u_des, lambda_des, Q[0], R[0], S[0]); + EXPECT_NEAR(actual, expected, 1e-12); // State, input, force costs + + // Test any mismatched dimensions throw errors. + std::vector x_wrong(3, VectorXd::Zero(1)); + ASSERT_ANY_THROW( + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(x_wrong, x_des, Q)); + + std::vector u_wrong(2, VectorXd::Zero(1)); + ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + x, u_wrong, x_des, u_des, Q, R)); + + std::vector lambda_wrong(2, VectorXd::Zero(1)); + ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + x, u, lambda_wrong, x_des, u_des, lambda_des, Q, R, S)); + + std::vector Q_wrong_size(2, MatrixXd::Zero(2, 2)); + ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + x, x_des, Q_wrong_size)); + + std::vector R_wrong_size(1, MatrixXd::Zero(2, 2)); + ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + x, u, x_des, u_des, Q, R_wrong_size)); + + std::vector S_wrong_size(1, MatrixXd::Zero(2, 2)); + ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + x, u, lambda, x_des, u_des, lambda_des, Q, R, S_wrong_size)); +} + +TEST_F(TrajectoryEvaluatorTest, SimulatePDControlWithLCSTest) { + const int n = 4; // 4 states: 2 positions, 2 velocities + const int k = 2; // 2 inputs + const int m = 1; + const int N = 3; + const double dt = 0.1; + + // Simple integrator dynamics: x_next = x + B*u + MatrixXd A = MatrixXd::Identity(n, n); + MatrixXd B = MatrixXd::Zero(n, k); + B(0, 0) = dt; + B(1, 1) = dt; + MatrixXd D = MatrixXd::Zero(n, m); + VectorXd d = VectorXd::Zero(n); + MatrixXd E = MatrixXd::Zero(m, n); + MatrixXd F = MatrixXd::Identity(m, m); + MatrixXd H = MatrixXd::Zero(m, k); + VectorXd c = VectorXd::Zero(m); + + LCS lcs(A, B, D, d, E, F, H, c, N, dt); + + // Create a planned trajectory + std::vector x_plan(N + 1, VectorXd::Zero(n)); + std::vector u_plan(N, VectorXd::Zero(k)); + + x_plan[0] << 0.0, 0.0, 0.0, 0.0; + x_plan[1] << 0.5, 0.5, 0.0, 0.0; + x_plan[2] << 0.8, 0.8, 0.0, 0.0; + x_plan[3] << 1.0, 1.0, 0.0, 0.0; + + u_plan[0] << 5.0, 5.0; + u_plan[1] << 3.0, 3.0; + u_plan[2] << 2.0, 2.0; + + // PD gains - Kp and Kd must have exactly k non-zero elements + VectorXd Kp = VectorXd::Zero(n); + VectorXd Kd = VectorXd::Zero(n); + Kp(0) = 10.0; + Kp(1) = 10.0; + Kd(2) = 1.0; + Kd(3) = 1.0; + + // Simulate with PD control + std::pair, std::vector> result = + TrajectoryEvaluator::SimulatePDControlWithLCS(x_plan, u_plan, Kp, Kd, + lcs); + + std::vector x_sim = result.first; + std::vector u_sim = result.second; + + EXPECT_EQ(x_sim.size(), x_plan.size()); + EXPECT_EQ(u_sim.size(), u_plan.size()); + EXPECT_TRUE(x_sim[0].isApprox(x_plan[0])); + + // Test with coarse and fine LCS (fine has 2x time resolution) + LCS fine_lcs(A, B / 2.0, D, d, E, F, H, c, N * 2, dt / 2); + std::pair, std::vector> result_fine = + TrajectoryEvaluator::SimulatePDControlWithLCS(x_plan, u_plan, Kp, Kd, lcs, + fine_lcs); + + std::vector x_sim_fine = result_fine.first; + std::vector u_sim_fine = result_fine.second; + + EXPECT_EQ(x_sim_fine.size(), x_plan.size()); + EXPECT_EQ(u_sim_fine.size(), u_plan.size()); + EXPECT_TRUE(x_sim_fine[0].isApprox(x_plan[0])); + + // Test error checking for mismatched dimensions + VectorXd Kp_wrong_size = VectorXd::Zero(n + 1); + ASSERT_ANY_THROW(TrajectoryEvaluator::SimulatePDControlWithLCS( + x_plan, u_plan, Kp_wrong_size, Kd, lcs)); + + VectorXd Kd_wrong_size = VectorXd::Zero(n - 1); + ASSERT_ANY_THROW(TrajectoryEvaluator::SimulatePDControlWithLCS( + x_plan, u_plan, Kp, Kd_wrong_size, lcs)); + + // Test wrong number of non-zero elements (must match k) + VectorXd Kp_wrong_count = VectorXd::Zero(n); + Kp_wrong_count(0) = 1.0; // Only 1 non-zero instead of k=2 + ASSERT_ANY_THROW(TrajectoryEvaluator::SimulatePDControlWithLCS( + x_plan, u_plan, Kp_wrong_count, Kd, lcs)); +} + +TEST_F(TrajectoryEvaluatorTest, ZeroOrderHoldAndDownsampleRoundTrip) { + std::vector coarse(2, VectorXd::Zero(2)); + coarse[0] << 1.0, 2.0; + coarse[1] << 3.0, 4.0; + + const int timestep_split = 3; + std::vector fine = + TrajectoryEvaluator::ZeroOrderHoldTrajectory(coarse, timestep_split); + EXPECT_EQ(fine.size(), coarse.size() * timestep_split); + for (int i = 0; i < timestep_split; ++i) { + EXPECT_TRUE(fine[i].isApprox(coarse[0])); + EXPECT_TRUE(fine[i + timestep_split].isApprox(coarse[1])); + } + + std::vector downsampled = + TrajectoryEvaluator::DownsampleTrajectory(fine, timestep_split); + EXPECT_EQ(downsampled.size(), coarse.size()); + EXPECT_TRUE(downsampled[0].isApprox(coarse[0])); + EXPECT_TRUE(downsampled[1].isApprox(coarse[1])); +} + +TEST_F(TrajectoryEvaluatorTest, MultiZeroOrderHoldAndDownsampleRoundTrip) { + std::vector x_coarse(3, VectorXd::Zero(2)); + x_coarse[0] << 1.0, 2.0; + x_coarse[1] << 3.0, 4.0; + x_coarse[2] << 5.0, 6.0; + std::vector u_coarse(2, VectorXd::Zero(1)); + u_coarse[0] << 7.0; + u_coarse[1] << 8.0; + + const int timestep_split = 3; + std::pair, std::vector> fine = + TrajectoryEvaluator::ZeroOrderHoldTrajectories(x_coarse, u_coarse, + timestep_split); + std::vector x_fine = fine.first; + std::vector u_fine = fine.second; + EXPECT_EQ(x_fine.size(), (x_coarse.size() - 1) * timestep_split + 1); + EXPECT_EQ(u_fine.size(), u_coarse.size() * timestep_split); + for (int i = 0; i < timestep_split; ++i) { + EXPECT_TRUE(x_fine[i].isApprox(x_coarse[0])); + EXPECT_TRUE(x_fine[i + timestep_split].isApprox(x_coarse[1])); + EXPECT_TRUE(u_fine[i].isApprox(u_coarse[0])); + EXPECT_TRUE(u_fine[i + timestep_split].isApprox(u_coarse[1])); + } + EXPECT_TRUE(x_fine.back().isApprox(x_coarse.back())); + + std::pair, std::vector> downsampled = + TrajectoryEvaluator::DownsampleTrajectories(x_fine, u_fine, + timestep_split); + std::vector x_downsampled = downsampled.first; + std::vector u_downsampled = downsampled.second; + EXPECT_EQ(x_downsampled.size(), 3); + EXPECT_EQ(x_downsampled.size(), x_coarse.size()); + EXPECT_EQ(u_downsampled.size(), 2); + EXPECT_EQ(u_downsampled.size(), u_coarse.size()); + EXPECT_TRUE(x_downsampled[0].isApprox(x_coarse[0])); + EXPECT_TRUE(x_downsampled[1].isApprox(x_coarse[1])); + EXPECT_TRUE(x_downsampled[2].isApprox(x_coarse[2])); + EXPECT_TRUE(u_downsampled[0].isApprox(u_coarse[0])); + EXPECT_TRUE(u_downsampled[1].isApprox(u_coarse[1])); + + // Test any mismatched dimensions throw errors. + std::vector x_coarse_wrong(2, VectorXd::Zero(3)); + ASSERT_ANY_THROW(TrajectoryEvaluator::ZeroOrderHoldTrajectories( + x_coarse_wrong, u_coarse, timestep_split)); + std::vector u_coarse_wrong(3, VectorXd::Zero(2)); + ASSERT_ANY_THROW(TrajectoryEvaluator::ZeroOrderHoldTrajectories( + x_coarse, u_coarse_wrong, timestep_split)); + std::vector x_fine_wrong(2, VectorXd::Zero(3)); + ASSERT_ANY_THROW(TrajectoryEvaluator::DownsampleTrajectories( + x_fine_wrong, u_fine, timestep_split)); + std::vector u_fine_wrong(3, VectorXd::Zero(2)); + ASSERT_ANY_THROW(TrajectoryEvaluator::DownsampleTrajectories( + x_fine, u_fine_wrong, timestep_split)); +} + +TEST_F(TrajectoryEvaluatorTest, + SimulateLCSOverTrajectoryMatchesLinearDynamics) { + const int n = 2; + const int k = 2; + const int m = 1; + const int N = 2; + const double dt = 0.1; + + MatrixXd A = MatrixXd::Identity(n, n); + MatrixXd B = MatrixXd::Identity(n, k); + MatrixXd D = MatrixXd::Zero(n, m); + VectorXd d = VectorXd::Zero(n); + MatrixXd E = MatrixXd::Zero(m, n); + MatrixXd F = MatrixXd::Identity(m, m); + MatrixXd H = MatrixXd::Zero(m, k); + VectorXd c = VectorXd::Zero(m); + + LCS lcs(A, B, D, d, E, F, H, c, N, dt); + + VectorXd x_init = VectorXd::Zero(n); + x_init << 1.0, 1.0; + std::vector u_plan(N, VectorXd::Zero(k)); + u_plan[0] << 1.0, 0.0; + u_plan[1] << 0.0, 2.0; + + std::vector x_sim = + TrajectoryEvaluator::SimulateLCSOverTrajectory(x_init, u_plan, lcs); + ASSERT_EQ(x_sim.size(), static_cast(N + 1)); + + VectorXd x1_expected = x_init + u_plan[0]; + VectorXd x2_expected = x1_expected + u_plan[1]; + EXPECT_TRUE(x_sim[0].isApprox(x_init)); + EXPECT_TRUE(x_sim[1].isApprox(x1_expected)); + EXPECT_TRUE(x_sim[2].isApprox(x2_expected)); + + // Test with a finer LCS (smaller dt) + LCS finer_lcs(A, B / 10.0, D, d, E, F, H, c, N * 10, dt / 10); + std::vector x_sim_from_finer = + TrajectoryEvaluator::SimulateLCSOverTrajectory(x_init, u_plan, lcs, + finer_lcs); + ASSERT_EQ(x_sim_from_finer.size(), static_cast(N + 1)); + EXPECT_TRUE(x_sim_from_finer[0].isApprox(x_init)); + EXPECT_TRUE(x_sim_from_finer[1].isApprox(x1_expected)); + EXPECT_TRUE(x_sim_from_finer[2].isApprox(x2_expected)); + + // Test any mismatched dimensions throw errors. + std::vector u_plan_wrong(2, VectorXd::Zero(3)); + ASSERT_ANY_THROW(TrajectoryEvaluator::SimulateLCSOverTrajectory( + x_init, u_plan_wrong, lcs)); + VectorXd x_init_wrong = VectorXd::Zero(3); + ASSERT_ANY_THROW(TrajectoryEvaluator::SimulateLCSOverTrajectory(x_init_wrong, + u_plan, lcs)); +} + template class C3CartpoleTypedTest : public testing::Test, public C3CartpoleProblem { protected: diff --git a/core/traj_eval.cc b/core/traj_eval.cc index 8cec7304..26082831 100644 --- a/core/traj_eval.cc +++ b/core/traj_eval.cc @@ -21,10 +21,11 @@ double TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( const vector& u_des, const vector& lambda_des, const vector& Q, const vector& R, const vector& S) { - DRAKE_DEMAND(x.size() == x_des.size() && x.size() == Q.size()); - DRAKE_DEMAND(u.size() == u_des.size() && u.size() == R.size()); - DRAKE_DEMAND(lambda.size() == lambda_des.size() && lambda.size() == S.size()); - DRAKE_DEMAND(x.size() - 1 == u.size() && u.size() == lambda.size()); + DRAKE_THROW_UNLESS(x.size() == x_des.size() && x.size() == Q.size()); + DRAKE_THROW_UNLESS(u.size() == u_des.size() && u.size() == R.size()); + DRAKE_THROW_UNLESS(lambda.size() == lambda_des.size() && + lambda.size() == S.size()); + DRAKE_THROW_UNLESS(x.size() - 1 == u.size() && u.size() == lambda.size()); int N = x.size() - 1; int n_x = x[0].size(); @@ -34,18 +35,18 @@ double TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( double cost = 0.0; for (int i = 0; i < N + 1; i++) { - DRAKE_DEMAND(x[i].size() == n_x && x_des[i].size() == n_x); - DRAKE_DEMAND(Q[i].rows() == n_x && Q[i].cols() == n_x); + DRAKE_THROW_UNLESS(x[i].size() == n_x && x_des[i].size() == n_x); + DRAKE_THROW_UNLESS(Q[i].rows() == n_x && Q[i].cols() == n_x); cost += (x[i] - x_des[i]).transpose() * Q[i] * (x[i] - x_des[i]); if (i < N) { - DRAKE_DEMAND(u[i].size() == n_u && u_des[i].size() == n_u); - DRAKE_DEMAND(R[i].rows() == n_u && R[i].cols() == n_u); + DRAKE_THROW_UNLESS(u[i].size() == n_u && u_des[i].size() == n_u); + DRAKE_THROW_UNLESS(R[i].rows() == n_u && R[i].cols() == n_u); cost += (u[i] - u_des[i]).transpose() * R[i] * (u[i] - u_des[i]); - DRAKE_DEMAND(lambda[i].size() == n_lambda && - lambda_des[i].size() == n_lambda); - DRAKE_DEMAND(S[i].rows() == n_lambda && S[i].cols() == n_lambda); + DRAKE_THROW_UNLESS(lambda[i].size() == n_lambda && + lambda_des[i].size() == n_lambda); + DRAKE_THROW_UNLESS(S[i].rows() == n_lambda && S[i].cols() == n_lambda); cost += (lambda[i] - lambda_des[i]).transpose() * S[i] * (lambda[i] - lambda_des[i]); } @@ -121,9 +122,9 @@ TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, // Ensure the Kp and Kd vectors encode the actuated position and velocity // indices within the state vector. - DRAKE_DEMAND(Kp.rows() == Kd.rows() && Kp.rows() == n_x); - DRAKE_DEMAND((Kp.array() != 0.0).count() == n_u); - DRAKE_DEMAND((Kd.array() != 0.0).count() == n_u); + DRAKE_THROW_UNLESS(Kp.rows() == Kd.rows() && Kp.rows() == n_x); + DRAKE_THROW_UNLESS((Kp.array() != 0.0).count() == n_u); + DRAKE_THROW_UNLESS((Kd.array() != 0.0).count() == n_u); MatrixXd Kp_mat = MatrixXd::Zero(n_u, n_x); MatrixXd Kd_mat = MatrixXd::Zero(n_u, n_x); int kp_i = 0; @@ -210,7 +211,7 @@ vector TrajectoryEvaluator::SimulateLCSOverTrajectory( ZeroOrderHoldTrajectory(u_plan, timestep_split); vector x_sim_fine = SimulateLCSOverTrajectory(x_init, u_plan_finer, fine_lcs); - return DownsampleTrajectory(x_sim_fine, timestep_split); + return DownsampleTrajectories(x_sim_fine, u_plan_finer, timestep_split).first; } vector TrajectoryEvaluator::SimulateLCSOverTrajectory( @@ -239,18 +240,21 @@ std::pair, vector> TrajectoryEvaluator::ZeroOrderHoldTrajectories(const vector& x_coarse, const vector& u_coarse, const int& timestep_split) { - DRAKE_DEMAND(x_coarse.size() == u_coarse.size() + 1); - vector x_fine = ZeroOrderHoldTrajectory(x_coarse, timestep_split); + DRAKE_THROW_UNLESS(x_coarse.size() == u_coarse.size() + 1); + vector x_fine = ZeroOrderHoldTrajectory( + vector(x_coarse.begin(), x_coarse.end() - 1), timestep_split); + x_fine.push_back(x_coarse.back()); vector u_fine = ZeroOrderHoldTrajectory(u_coarse, timestep_split); return std::make_pair(x_fine, u_fine); } vector TrajectoryEvaluator::DownsampleTrajectory( const vector& fine_traj, const int& timestep_split) { + DRAKE_THROW_UNLESS(fine_traj.size() % timestep_split == 0); int N = fine_traj.size() / timestep_split; // Downsample the fine trajectory. - vector coarse_traj(N + 1, VectorXd::Zero(fine_traj[0].size())); + vector coarse_traj(N, VectorXd::Zero(fine_traj[0].size())); for (int i = 0; i < N; i++) { coarse_traj[i] = fine_traj[i * timestep_split]; } @@ -261,23 +265,25 @@ std::pair, vector> TrajectoryEvaluator::DownsampleTrajectories(const vector& x_fine, const vector& u_fine, const int& timestep_split) { - DRAKE_DEMAND(x_fine.size() == u_fine.size() + 1); - vector x_coarse = DownsampleTrajectory(x_fine, timestep_split); + DRAKE_THROW_UNLESS(x_fine.size() == u_fine.size() + 1); + vector x_coarse = DownsampleTrajectory( + vector(x_fine.begin(), x_fine.end() - 1), timestep_split); + x_coarse.push_back(x_fine.back()); vector u_coarse = DownsampleTrajectory(u_fine, timestep_split); return std::make_pair(x_coarse, u_coarse); } int TrajectoryEvaluator::CheckCoarseAndFineLCSCompatibility( const LCS& coarse_lcs, const LCS& fine_lcs) { - DRAKE_DEMAND(coarse_lcs.num_states() == fine_lcs.num_states()); - DRAKE_DEMAND(coarse_lcs.num_inputs() == fine_lcs.num_inputs()); - DRAKE_DEMAND(coarse_lcs.N() <= fine_lcs.N()); - DRAKE_DEMAND(coarse_lcs.dt() >= fine_lcs.dt()); - DRAKE_DEMAND(coarse_lcs.N() * coarse_lcs.dt() == - fine_lcs.N() * fine_lcs.dt()); + DRAKE_THROW_UNLESS(coarse_lcs.num_states() == fine_lcs.num_states()); + DRAKE_THROW_UNLESS(coarse_lcs.num_inputs() == fine_lcs.num_inputs()); + DRAKE_THROW_UNLESS(coarse_lcs.N() <= fine_lcs.N()); + DRAKE_THROW_UNLESS(coarse_lcs.dt() >= fine_lcs.dt()); + DRAKE_THROW_UNLESS(coarse_lcs.N() * coarse_lcs.dt() == + fine_lcs.N() * fine_lcs.dt()); int timestep_split = fine_lcs.N() / coarse_lcs.N(); - DRAKE_DEMAND(fine_lcs.dt() * timestep_split == coarse_lcs.dt()); - DRAKE_DEMAND(coarse_lcs.N() * timestep_split == fine_lcs.N()); + DRAKE_THROW_UNLESS(fine_lcs.dt() * timestep_split == coarse_lcs.dt()); + DRAKE_THROW_UNLESS(coarse_lcs.N() * timestep_split == fine_lcs.N()); return timestep_split; } @@ -285,13 +291,13 @@ void TrajectoryEvaluator::CheckLCSAndTrajectoryCompatibility( const LCS& lcs, const std::vector& x, const std::vector& u, const std::vector& lambda) { - DRAKE_DEMAND(lcs.N() == x.size() - 1 && lcs.N() == u.size() && - lcs.N() == lambda.size()); + DRAKE_THROW_UNLESS(lcs.N() == x.size() - 1 && lcs.N() == u.size() && + lcs.N() == lambda.size()); for (int i = 0; i < lcs.N() + 1; i++) { - DRAKE_DEMAND(x[i].size() == lcs.num_states()); + DRAKE_THROW_UNLESS(x[i].size() == lcs.num_states()); if (i < lcs.N()) { - DRAKE_DEMAND(u[i].size() == lcs.num_inputs()); - DRAKE_DEMAND(lambda[i].size() == lcs.num_lambdas()); + DRAKE_THROW_UNLESS(u[i].size() == lcs.num_inputs()); + DRAKE_THROW_UNLESS(lambda[i].size() == lcs.num_lambdas()); } } } diff --git a/core/traj_eval.h b/core/traj_eval.h index d9f093b5..a22afa09 100644 --- a/core/traj_eval.h +++ b/core/traj_eval.h @@ -13,13 +13,6 @@ namespace traj_eval { /// Utility class for trajectory evaluation computations and simulations. class TrajectoryEvaluator { public: - enum CostComputationType : uint8_t { - SIM_IMPEDANCE = 1, - SIM_OBJECT_LCS_ROBOT_PLAN = 2 - }; - - static double EvaluateTrajectoryCost(); - /// Compute the quadratic cost of a trajectory of states, inputs, and contact /// forces, with respect to a desired trajectory and quadratic cost matrices. /// Cost = Sum_{i = 0, ... N-1} @@ -118,11 +111,15 @@ class TrajectoryEvaluator { /// a different LCS with a finer time discretization. The LCSs must be /// compatible with each other, i.e. the finer horizon must be an integer /// multiple of the coarser horizon, and dt*N must be the same for both LCSs. + /// The state trajectory (which was N+1 in length) becomes + /// (N*timestep_split)+1 in length, and the input trajectory (which was N in + /// length) becomes N*timestep_split in length. static std::pair, std::vector> ZeroOrderHoldTrajectories(const std::vector& x_coarse, const std::vector& u_coarse, const int& timestep_split); /// Same as above but only one trajectory is provided and processed. + /// Trajectories of length N become trajectories of length N*timestep_split. static std::vector ZeroOrderHoldTrajectory( const std::vector& coarse_traj, const int& timestep_split); From c50db1a1492a3cbea2a53386fbb287a3b6b73200 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Tue, 3 Mar 2026 13:03:00 -0500 Subject: [PATCH 06/17] Clean outdated comments; match function comment formatting to rest of code --- bindings/pyc3/c3_py.cc | 8 -- core/c3.cc | 116 ------------------------- core/c3.h | 42 --------- core/test/core_test.cc | 2 +- core/traj_eval.h | 188 ++++++++++++++++++++++++++++------------- 5 files changed, 132 insertions(+), 224 deletions(-) diff --git a/bindings/pyc3/c3_py.cc b/bindings/pyc3/c3_py.cc index 99ee8e25..703c58f6 100644 --- a/bindings/pyc3/c3_py.cc +++ b/bindings/pyc3/c3_py.cc @@ -73,11 +73,6 @@ PYBIND11_MODULE(c3, m) { .value("FORCE", ConstraintVariable::FORCE) .export_values(); - // py::enum_(m, "CostComputationType") - // .value("STATE", CostComputationType::SIM_IMPEDANCE) - // .value("INPUT", CostComputationType::SIM_OBJECT_LCS_ROBOT_PLAN) - // .export_values(); // TODO @bibit: remove - py::class_(m, "C3") .def(py::init&, const C3Options&>(), @@ -115,9 +110,6 @@ PYBIND11_MODULE(c3, m) { .def("GetLinearConstraints", &C3::GetLinearConstraints, py::return_value_policy::copy) .def("SetSolverOptions", &C3::SetSolverOptions, py::arg("options")) - // .def("UpdateCostLCS", &C3::UpdateCostLCS, py::arg("cost_lcs")) - // .def("CalculateCost", &C3::CalculateCost, py::arg("cost_type"), - // py::arg("Kp"), py::arg("Kd")) // TODO @bibit: remove .def("GetFullSolution", &C3::GetFullSolution) .def("GetStateSolution", &C3::GetStateSolution) .def("GetForceSolution", &C3::GetForceSolution) diff --git a/core/c3.cc b/core/c3.cc index 152c822e..8d837081 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -51,7 +51,6 @@ C3::C3(const LCS& lcs, const CostMatrices& costs, n_u_(lcs.num_inputs()), n_z_(z_size), lcs_(lcs), - // cost_lcs_(lcs), // TODO @bibit: remove cost_matrices_(costs), x_desired_(x_desired), options_(options), @@ -212,121 +211,6 @@ void C3::ScaleLCS() { AnDn_ = lcs_.ScaleComplementarityDynamics(); } -/* TODO @bibit: remove -void C3::UpdateCostLCS(const LCS& cost_lcs) { - DRAKE_DEMAND(lcs_.num_states() == cost_lcs.num_states()); - DRAKE_DEMAND(lcs_.num_inputs() == cost_lcs.num_inputs()); - DRAKE_DEMAND(lcs_.N() <= cost_lcs.N()); - DRAKE_DEMAND(lcs_.dt() >= cost_lcs.dt()); - DRAKE_DEMAND(lcs_.N() * lcs_.dt() == cost_lcs.N() * cost_lcs.dt()); - - int timestep_split = cost_lcs.N() / lcs_.N(); - DRAKE_DEMAND(cost_lcs.dt() * timestep_split == lcs_.dt()); - DRAKE_DEMAND(lcs_.N() * timestep_split == cost_lcs.N()); - - // Update the stored LCS object. - cost_lcs_ = cost_lcs; -} - -std::pair> C3::CalculateCost( - const CostComputationType& cost_type, const VectorXd& Kp, - const VectorXd& Kd) { - DRAKE_DEMAND(cost_type == CostComputationType::SIM_IMPEDANCE || - cost_type == CostComputationType::SIM_OBJECT_LCS_ROBOT_PLAN); - - int timestep_split = cost_lcs_.N() / lcs_.N(); - - // Get the C3 plan. - vector planned_state_trajectory_coarse = GetStateSolution(); - vector planned_input_trajectory_coarse = GetInputSolution(); - - // Compute the new trajectory used for cost evaluation. - vector state_trajectory(N_ * timestep_split + 1, - VectorXd::Zero(n_x_)); - vector input_trajectory(N_ * timestep_split, VectorXd::Zero(n_u_)); - if (cost_type == CostComputationType::SIM_IMPEDANCE) { - // Ensure the Kp and Kd vectors encode the actuated position and velocity - // indices within the state vector. - DRAKE_DEMAND(Kp.rows() == Kd.rows() && Kp.rows() == n_x_); - DRAKE_DEMAND((Kp.array() != 0.0).count() == n_u_); - DRAKE_DEMAND((Kd.array() != 0.0).count() == n_u_); - MatrixXd Kp_mat = MatrixXd::Zero(n_u_, n_x_); - MatrixXd Kd_mat = MatrixXd::Zero(n_u_, n_x_); - int kp_i = 0; - int kd_i = 0; - for (int i = 0; i < n_x_; ++i) { - if (Kp(i) != 0) { - Kp_mat(kp_i, i) = Kp(i); - kp_i++; - } - if (Kd(i) != 0) { - Kd_mat(kd_i, i) = Kd(i); - kd_i++; - } - } - - state_trajectory[0] = planned_state_trajectory_coarse[0]; - for (int i = 0; i < N_ * timestep_split; i++) { - VectorXd x = planned_state_trajectory_coarse.at(i / timestep_split); - VectorXd u = planned_input_trajectory_coarse.at(i / timestep_split); - VectorXd x_des = x_desired_.at(i / timestep_split); - VectorXd x_curr = state_trajectory.at(i); - input_trajectory[i] = - planned_input_trajectory_coarse.at(i / timestep_split); - input_trajectory[i] += Kp_mat * (x - x_curr) + Kd_mat * (x - x_curr); - state_trajectory[i + 1] = cost_lcs_.Simulate(x_curr, input_trajectory[i]); - } - } else if (cost_type == CostComputationType::SIM_OBJECT_LCS_ROBOT_PLAN) { - state_trajectory[0] = planned_state_trajectory_coarse[0]; - for (int i = 0; i < N_ * timestep_split; i++) { - VectorXd x_curr = state_trajectory.at(i); - input_trajectory[i] = - planned_input_trajectory_coarse.at(i / timestep_split); - state_trajectory[i + 1] = cost_lcs_.Simulate(x_curr, input_trajectory[i]); - } - // Replace the simulated robot states with the planned ones. - for (int i = 0; i < N_ * timestep_split; i++) { - state_trajectory[i].segment(0, n_u_) = - planned_state_trajectory_coarse.at(i / timestep_split) - .segment(0, n_u_); - } - state_trajectory[N_ * timestep_split].segment(0, n_u_) = - cost_lcs_ - .Simulate(state_trajectory[N_ * timestep_split - 1], - input_trajectory[N_ * timestep_split - 1]) - .segment(0, n_u_); - } - - // Downsample the state trajectory to match the C3 plan timesteps. - vector state_trajectory_downsampled(N_ + 1, VectorXd::Zero(n_x_)); - vector input_trajectory_downsampled(N_, VectorXd::Zero(n_u_)); - for (int i = 0; i < N_ * timestep_split; i += timestep_split) { - state_trajectory_downsampled[i / timestep_split] = state_trajectory[i]; - input_trajectory_downsampled[i / timestep_split] = input_trajectory[i]; - } - state_trajectory_downsampled[N_] = state_trajectory[N_ * timestep_split]; - - // Compute the cost based on the downsampled trajectory. - // NOTE: this doesn't handle (u-u_prev)^T R (u-u_prev) - double cost = 0.0; - for (int i = 0; i < N_ + 1; i++) { - VectorXd x_des = x_desired_.at(i); - cost += (state_trajectory_downsampled[i] - x_des).transpose() * - cost_matrices_.Q_evaluation.at(i) * - (state_trajectory_downsampled[i] - x_des); - if (i < N_) { - cost += input_trajectory_downsampled[i].transpose() * - cost_matrices_.R_evaluation.at(i) * - input_trajectory_downsampled[i]; - } - } - - std::pair> cost_and_trajectory( - cost, state_trajectory_downsampled); - return cost_and_trajectory; -} -*/ - void C3::UpdateLCS(const LCS& lcs) { DRAKE_DEMAND(lcs_.HasSameDimensionsAs(lcs)); diff --git a/core/c3.h b/core/c3.h index 2a083063..498a8438 100644 --- a/core/c3.h +++ b/core/c3.h @@ -18,12 +18,6 @@ typedef drake::solvers::Binding enum ConstraintVariable : uint8_t { STATE = 1, INPUT = 2, FORCE = 3 }; -// TODO @bibit: remove -// enum CostComputationType : uint8_t { -// SIM_IMPEDANCE = 1, -// SIM_OBJECT_LCS_ROBOT_PLAN = 2 -// }; - class C3 { public: /*! @@ -197,41 +191,6 @@ class C3 { prog_.SetSolverOptions(options); } - /** - * TODO @bibit: remove - * @brief Update the cost LCS used for cost evaluation in CalculateCost. - * - * @param cost_lcs the new LCS to be used for cost evaluation. Must have the - * same state and input dimensions as the LCS used for planning. The horizon - * and timestep can differ, but must be compatible in that cost_lcs.N() must - * be a multiple of lcs_.N(), and N*dt must be the same for both LCS objects. - * This allows for evaluating costs with a higher temporal resolution than the - * planning LCS. Can differ in lambda dimensions, if higher contact resolution - * is desired for evaluation. - */ - // void UpdateCostLCS(const LCS& cost_lcs); - - /** - * TODO @bibit: remove - * @brief Calculate the cost of the current solution, using the provided cost - * type and optionally provided Kp and Kd values for the cost calculation. - * - * @param cost_type - * @param Kp Proportional gain values for the cost calculation, used only if - * the cost type requires them. If used, the length of Kp should be n_x_ and - * the number of non-zero elements should be n_u_. This encodes which state - * indices are positions associated with a robot actuator. - * @param Kd Derivative gain values for the cost calculation, used only if - * the cost type requires them. If used, the length of Kd should be n_x_ and - * the number of non-zero elements should be n_u_. This encodes which state - * indices are velocities associated with a robot actuator. - * @return A pair consisting of the cost (as a double) and a vector of the - * state trajectory associated with the cost. - */ - // std::pair> CalculateCost( - // const CostComputationType& cost_type, const Eigen::VectorXd& Kp = {}, - // const Eigen::VectorXd& Kd = {}); - std::vector GetFullSolution() { return *z_sol_; } std::vector GetStateSolution() { return *x_sol_; } std::vector GetForceSolution() { return *lambda_sol_; } @@ -431,7 +390,6 @@ class C3 { int admm_iteration, bool is_final_solve); LCS lcs_; - // LCS cost_lcs_; // TODO @bibit: remove double AnDn_ = 1.0; // Scaling factor for lambdas CostMatrices cost_matrices_; std::vector x_desired_; diff --git a/core/test/core_test.cc b/core/test/core_test.cc index 22d01452..e07b90d5 100644 --- a/core/test/core_test.cc +++ b/core/test/core_test.cc @@ -45,7 +45,7 @@ using namespace c3; * | CreatePlaceholderLCS | DONE | * | WarmStartSmokeTest | DONE | * | # of regression tests | 2 | - * | traj_eval | @bibit | + * | traj_eval | DONE | * * It also has an E2E test for ensuring the "Solve()" function and other * internal functions are working as expected. However, the E2E takes about 120s diff --git a/core/traj_eval.h b/core/traj_eval.h index a22afa09..bd0273b9 100644 --- a/core/traj_eval.h +++ b/core/traj_eval.h @@ -13,13 +13,28 @@ namespace traj_eval { /// Utility class for trajectory evaluation computations and simulations. class TrajectoryEvaluator { public: - /// Compute the quadratic cost of a trajectory of states, inputs, and contact - /// forces, with respect to a desired trajectory and quadratic cost matrices. - /// Cost = Sum_{i = 0, ... N-1} - /// (x_i-x_des_i)^T Q_i (x_i-x_des_i) + - /// (u_i-u_des_i)^T R_i (u_i-u_des_i) + - /// (lambda_i-lambda_des_i)^T S_i (lambda_i-lambda_des_i) - /// + (x_N-x_des_N)^T Q_N (x_N-x_des_N) + /** + * @brief Computes the quadratic cost of a trajectory of states, inputs, and + * contact forces, with respect to a desired trajectory and quadratic cost + * matrices. + * + * Cost = Sum_{i = 0, ... N-1} + * (x_i-x_des_i)^T Q_i (x_i-x_des_i) + + * (u_i-u_des_i)^T R_i (u_i-u_des_i) + + * (lambda_i-lambda_des_i)^T S_i (lambda_i-lambda_des_i) + * + (x_N-x_des_N)^T Q_N (x_N-x_des_N) + * + * @param x Trajectory of length N+1 of state vectors + * @param u Trajectory of length N of input vectors + * @param lambda Trajectory of length N of contact force vectors + * @param x_des Trajectory of length N+1 of desired state vectors + * @param u_des Trajectory of length N of desired input vectors + * @param lambda_des Trajectory of length N of desired contact force vectors + * @param Q Trajectory of length N+1 of state cost matrices + * @param R Trajectory of length N of input cost matrices + * @param S Trajectory of length N of contact force cost matrices + * @return The cost + */ static double ComputeQuadraticTrajectoryCost( const std::vector& x, const std::vector& u, @@ -30,7 +45,7 @@ class TrajectoryEvaluator { const std::vector& Q, const std::vector& R, const std::vector& S); - /// Special case: cost does not depend on the contact forces. + /** @brief Special case: cost does not depend on the contact forces. */ static double ComputeQuadraticTrajectoryCost( const std::vector& x, const std::vector& u, @@ -38,12 +53,13 @@ class TrajectoryEvaluator { const std::vector& u_des, const std::vector& Q, const std::vector& R); - /// Special case: cost does not depend on the inputs or contact forces. + /** @brief Special case: cost does not depend on the inputs or contact forces. + */ static double ComputeQuadraticTrajectoryCost( const std::vector& x, const std::vector& x_des, const std::vector& Q); - /// Special case: cost matrices are the same across all time steps. + /** @brief Special case: cost matrices are the same across all time steps. */ static double ComputeQuadraticTrajectoryCost( const std::vector& x, const std::vector& u, @@ -52,113 +68,171 @@ class TrajectoryEvaluator { const std::vector& u_des, const std::vector& lambda_des, const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R, const Eigen::MatrixXd& S); - /// Special case: cost does not depend on the contact forces, and cost - /// matrices are the same across all time steps. + /** @brief Special case: cost does not depend on the contact forces, and cost + * matrices are the same across all time steps. */ static double ComputeQuadraticTrajectoryCost( const std::vector& x, const std::vector& u, const std::vector& x_des, const std::vector& u_des, const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R); - /// Special case: cost does not depend on the inputs or contact forces, and - /// cost matrices are the same across all time steps. + /** @brief Special case: cost does not depend on the inputs or contact forces, + * and cost matrices are the same across all time steps. */ static double ComputeQuadraticTrajectoryCost( const std::vector& x, const std::vector& x_des, const Eigen::MatrixXd& Q); - /// From a planned trajectory of states and inputs and sets of Kp and Kd - /// gains, use an LCS to simulate tracking the plan with PD control (with - /// feedforward control), and return the resulting trajectory of actual states - /// and inputs. + /** + * @brief From a planned trajectory of states and inputs and sets of Kp and Kd + * gains, use an LCS to simulate tracking the plan with PD control (with + * feedforward control), and return the resulting trajectory of actual states + * and inputs. + * + * @param x_plan Planned trajectory of states (length N+1) + * @param u_plan Planned trajectory of inputs (length N) + * @param Kp Proportional gains (length n_x, with exactly k non-zero entries) + * @param Kd Derivative gains (length n_x, with exactly k non-zero entries) + * @param lcs LCS system to simulate + * @return Pair of (simulated states, simulated inputs) + */ static std::pair, std::vector> SimulatePDControlWithLCS(const std::vector& x_plan, const std::vector& u_plan, const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd, const LCS& lcs); - /// Special case: simulate plans from a coarser LCS with a finer LCS. The - /// returned trajectory is downsampled back to be compatible with the coarser - /// LCS. The two LCSs must be compatible with each other, and the state and - /// input plans must be compatible with the coarser LCS. + /** + * @brief Special case: simulate plans from a coarser LCS with a finer LCS. + * The returned trajectory is downsampled back to be compatible with the + * coarser LCS. The two LCSs must be compatible with each other, and the state + * and input plans must be compatible with the coarser LCS. + */ static std::pair, std::vector> SimulatePDControlWithLCS(const std::vector& x_plan, const std::vector& u_plan, const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd, const LCS& coarse_lcs, const LCS& fine_lcs); - /// Simulate an LCS forward from an initial condition over a trajectory of - /// inputs, and return the resulting trajectory of states. + /** + * @brief Simulate an LCS forward from an initial condition over a trajectory + * of inputs, and return the resulting trajectory of states. + * + * @param x_init Initial state + * @param u_plan Trajectory of inputs (length N) + * @param lcs LCS system to simulate + * @return Trajectory of simulated states (length N+1) + */ static std::vector SimulateLCSOverTrajectory( const Eigen::VectorXd& x_init, const std::vector& u_plan, const LCS& lcs); - /// Special case: a full trajectory of states is provided, but only the - /// initial state is used for simulation. + /** @brief Special case: a full trajectory of states is provided, but only the + * initial state is used for simulation. */ static std::vector SimulateLCSOverTrajectory( const std::vector& x_plan, const std::vector& u_plan, const LCS& lcs); - /// Special case: simulate a trajectory from a coarser LCS with a finer LCS. + /** + * @brief Special case: simulate plans from a coarser LCS with a finer LCS. + * The returned trajectory is downsampled back to be compatible with the + * coarser LCS. The two LCSs must be compatible with each other, and the input + * plan must be compatible with the coarser LCS. + */ static std::vector SimulateLCSOverTrajectory( const Eigen::VectorXd& x_init, const std::vector& u_plan, const LCS& coarse_lcs, const LCS& fine_lcs); - /// Special case: simulate a trajectory from a coarser LCS with a finer LCS, - /// where a full trajectory of states is provided but only the initial state - /// is used. + /** + * @brief Special case: simulate a trajectory from a coarser LCS with a finer + * LCS, where a full trajectory of states is provided but only the initial + * state is used. + */ static std::vector SimulateLCSOverTrajectory( const std::vector& x_plan, const std::vector& u_plan, const LCS& coarse_lcs, const LCS& fine_lcs); - /// Zero order hold a trajectory compatible with one LCS to be compatible with - /// a different LCS with a finer time discretization. The LCSs must be - /// compatible with each other, i.e. the finer horizon must be an integer - /// multiple of the coarser horizon, and dt*N must be the same for both LCSs. - /// The state trajectory (which was N+1 in length) becomes - /// (N*timestep_split)+1 in length, and the input trajectory (which was N in - /// length) becomes N*timestep_split in length. + /** + * @brief Zero order hold a trajectory compatible with one LCS to be + * compatible with a different LCS with a finer time discretization. The LCSs + * must be compatible with each other, i.e. the finer horizon must be an + * integer multiple of the coarser horizon, and dt*N must be the same for both + * LCSs. The state trajectory (which was N+1 in length) becomes + * (N*timestep_split)+1 in length, and the input trajectory (which was N in + * length) becomes N*timestep_split in length. + * + * @param x_coarse Coarse state trajectory (length N+1) + * @param u_coarse Coarse input trajectory (length N) + * @param timestep_split Integer multiple for time discretization + * @return Pair of 1) fine state trajectory (length (N*timestep_split)+1) and + * 2) fine input trajectory (length N*timestep_split) + */ static std::pair, std::vector> ZeroOrderHoldTrajectories(const std::vector& x_coarse, const std::vector& u_coarse, const int& timestep_split); - /// Same as above but only one trajectory is provided and processed. - /// Trajectories of length N become trajectories of length N*timestep_split. + /** + * @brief Same as above but only one trajectory is provided and processed. + * Trajectories of length N become trajectories of length N*timestep_split. + */ static std::vector ZeroOrderHoldTrajectory( const std::vector& coarse_traj, const int& timestep_split); - /// The reverse of ZeroOrderHoldTrajectoryToFinerLCS: downsample a trajectory - /// compatible with a finer LCS to be compatible with a coarser LCS. The same - /// compatibility requirements apply as for ZeroOrderHoldTrajectoryToFinerLCS. + /** + * @brief The reverse of ZeroOrderHoldTrajectoryToFinerLCS: downsample a + * trajectory compatible with a finer LCS to be compatible with a coarser LCS. + * The same compatibility requirements apply as for + * ZeroOrderHoldTrajectoryToFinerLCS. + * + * @param x_fine Fine state trajectory (length (N*timestep_split)+1) + * @param u_fine Fine input trajectory (length N*timestep_split) + * @param timestep_split Integer multiple for time discretization + * @return Pair of 1) coarse state trajectory (length N+1) and 2) coarse input + * trajectory (length N) + */ static std::pair, std::vector> DownsampleTrajectories(const std::vector& x_fine, const std::vector& u_fine, const int& timestep_split); - /// Same as above but only one trajectory is provided and processed. + /** @brief Same as above but only one trajectory is provided and processed. */ static std::vector DownsampleTrajectory( const std::vector& fine_traj, const int& timestep_split); - /// Check that two LCSs are compatible with each other, i.e. that the finer - /// horizon is an integer multiple of the coarser horizon, and that dt*N is - /// the same for both LCSs. Returns the integer multiple by which the finer - /// horizon is larger than the coarser horizon (i.e. the number of finer time - /// steps per coarser time step). NOTE: This function does not assert that the - /// LCSs' lambda dimensions are the same, since it may be desirable to use - /// different contact pairs for each LCS. The state and input dimensions must - /// be the same. + /** + * @brief Check that two LCSs are compatible with each other, i.e. that the + * finer horizon is an integer multiple of the coarser horizon, and that dt*N + * is the same for both LCSs. Returns the integer multiple by which the finer + * horizon is larger than the coarser horizon (i.e. the number of finer time + * steps per coarser time step). NOTE: This function does not assert that the + * LCSs' lambda dimensions are the same, since it may be desirable to use + * different contact pairs for each LCS. The state and input dimensions must + * be the same. + * + * @param coarse_lcs LCS with coarser time discretization + * @param fine_lcs LCS with finer time discretization + * @return Integer multiple by which fine_lcs.N() is larger than + * coarse_lcs.N() + */ static int CheckCoarseAndFineLCSCompatibility(const LCS& coarse_lcs, const LCS& fine_lcs); - /// Check that a trajectory of states, inputs, and contact forces is - /// compatible with an LCS, i.e. that the trajectory has the correct number of - /// time steps and that the dimensions of the states, inputs, and contact - /// forces match the dimensions of the LCS. + /** + * @brief Check that a trajectory of states, inputs, and contact forces is + * compatible with an LCS, i.e. that the trajectory has the correct number of + * time steps and that the dimensions of the states, inputs, and contact + * forces match the dimensions of the LCS. + * + * @param lcs LCS system + * @param x State trajectory (length N+1) + * @param u Input trajectory (length N) + * @param lambda Contact force trajectory (length N) + */ static void CheckLCSAndTrajectoryCompatibility( const LCS& lcs, const std::vector& x, const std::vector& u, const std::vector& lambda); - /// Special case: no lambdas provided. + /** @brief Special case: no lambdas provided. */ static void CheckLCSAndTrajectoryCompatibility( const LCS& lcs, const std::vector& x, const std::vector& u); - /// Special case: no inputs or lambdas provided. + /** @brief Special case: no inputs or lambdas provided. */ static void CheckLCSAndTrajectoryCompatibility( const LCS& lcs, const std::vector& x); }; From 21f98bb20548bd5dafa11650605ee88db523cf24 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Tue, 3 Mar 2026 15:24:10 -0500 Subject: [PATCH 07/17] Switch quadratic trajectory cost implementation to more flexible variadic approach --- core/test/core_test.cc | 20 ++++---- core/traj_eval.cc | 88 --------------------------------- core/traj_eval.h | 109 +++++++++++++++++++---------------------- 3 files changed, 60 insertions(+), 157 deletions(-) diff --git a/core/test/core_test.cc b/core/test/core_test.cc index e07b90d5..5bfda397 100644 --- a/core/test/core_test.cc +++ b/core/test/core_test.cc @@ -358,13 +358,13 @@ TEST_F(TrajectoryEvaluatorTest, QuadraticCostMatchesManual) { EXPECT_NEAR(actual, expected, 1e-12); // State costs expected += 3.0 * 1.0 * 1.0; - actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(x, u, x_des, - u_des, Q, R); + actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(x, x_des, Q, u, + u_des, R); EXPECT_NEAR(actual, expected, 1e-12); // State and input costs expected += 5.0 * 0.25 * 0.25; actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( - x, u, lambda, x_des, u_des, lambda_des, Q, R, S); + x, x_des, Q, u, u_des, R, lambda, lambda_des, S); EXPECT_NEAR(actual, expected, 1e-12); // State, input, force costs // Test with repeated cost matrices across time steps @@ -375,13 +375,13 @@ TEST_F(TrajectoryEvaluatorTest, QuadraticCostMatchesManual) { EXPECT_NEAR(actual, expected, 1e-12); // State costs expected += 3.0 * 1.0 * 1.0; - actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( - x, u, x_des, u_des, Q[0], R[0]); + actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(x, x_des, Q[0], + u, u_des, R[0]); EXPECT_NEAR(actual, expected, 1e-12); // State and input costs expected += 5.0 * 0.25 * 0.25; actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( - x, u, lambda, x_des, u_des, lambda_des, Q[0], R[0], S[0]); + x, x_des, Q[0], u, u_des, R[0], lambda, lambda_des, S[0]); EXPECT_NEAR(actual, expected, 1e-12); // State, input, force costs // Test any mismatched dimensions throw errors. @@ -391,11 +391,11 @@ TEST_F(TrajectoryEvaluatorTest, QuadraticCostMatchesManual) { std::vector u_wrong(2, VectorXd::Zero(1)); ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( - x, u_wrong, x_des, u_des, Q, R)); + x, x_des, Q, u_wrong, u_des, R)); std::vector lambda_wrong(2, VectorXd::Zero(1)); ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( - x, u, lambda_wrong, x_des, u_des, lambda_des, Q, R, S)); + x, x_des, Q, u, u_des, R, lambda_wrong, lambda_des, S)); std::vector Q_wrong_size(2, MatrixXd::Zero(2, 2)); ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( @@ -403,11 +403,11 @@ TEST_F(TrajectoryEvaluatorTest, QuadraticCostMatchesManual) { std::vector R_wrong_size(1, MatrixXd::Zero(2, 2)); ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( - x, u, x_des, u_des, Q, R_wrong_size)); + x, x_des, Q, u, u_des, R_wrong_size)); std::vector S_wrong_size(1, MatrixXd::Zero(2, 2)); ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( - x, u, lambda, x_des, u_des, lambda_des, Q, R, S_wrong_size)); + x, x_des, Q, u, u_des, R, lambda, lambda_des, S_wrong_size)); } TEST_F(TrajectoryEvaluatorTest, SimulatePDControlWithLCSTest) { diff --git a/core/traj_eval.cc b/core/traj_eval.cc index 26082831..afeddc3f 100644 --- a/core/traj_eval.cc +++ b/core/traj_eval.cc @@ -15,94 +15,6 @@ using Eigen::MatrixXd; using Eigen::VectorXd; using std::vector; -double TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( - const vector& x, const vector& u, - const vector& lambda, const vector& x_des, - const vector& u_des, const vector& lambda_des, - const vector& Q, const vector& R, - const vector& S) { - DRAKE_THROW_UNLESS(x.size() == x_des.size() && x.size() == Q.size()); - DRAKE_THROW_UNLESS(u.size() == u_des.size() && u.size() == R.size()); - DRAKE_THROW_UNLESS(lambda.size() == lambda_des.size() && - lambda.size() == S.size()); - DRAKE_THROW_UNLESS(x.size() - 1 == u.size() && u.size() == lambda.size()); - - int N = x.size() - 1; - int n_x = x[0].size(); - int n_u = u[0].size(); - int n_lambda = lambda[0].size(); - - double cost = 0.0; - - for (int i = 0; i < N + 1; i++) { - DRAKE_THROW_UNLESS(x[i].size() == n_x && x_des[i].size() == n_x); - DRAKE_THROW_UNLESS(Q[i].rows() == n_x && Q[i].cols() == n_x); - cost += (x[i] - x_des[i]).transpose() * Q[i] * (x[i] - x_des[i]); - - if (i < N) { - DRAKE_THROW_UNLESS(u[i].size() == n_u && u_des[i].size() == n_u); - DRAKE_THROW_UNLESS(R[i].rows() == n_u && R[i].cols() == n_u); - cost += (u[i] - u_des[i]).transpose() * R[i] * (u[i] - u_des[i]); - - DRAKE_THROW_UNLESS(lambda[i].size() == n_lambda && - lambda_des[i].size() == n_lambda); - DRAKE_THROW_UNLESS(S[i].rows() == n_lambda && S[i].cols() == n_lambda); - cost += (lambda[i] - lambda_des[i]).transpose() * S[i] * - (lambda[i] - lambda_des[i]); - } - } - - return cost; -} - -double TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( - const vector& x, const vector& u, - const vector& x_des, const vector& u_des, - const vector& Q, const vector& R) { - return ComputeQuadraticTrajectoryCost( - x, u, vector(u.size(), VectorXd::Zero(0)), x_des, u_des, - vector(u.size(), VectorXd::Zero(0)), Q, R, - vector(u.size(), MatrixXd::Zero(0, 0))); -} - -double TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( - const vector& x, const vector& x_des, - const vector& Q) { - return ComputeQuadraticTrajectoryCost( - x, vector(x.size() - 1, VectorXd::Zero(0)), - vector(x.size() - 1, VectorXd::Zero(0)), x_des, - vector(x.size() - 1, VectorXd::Zero(0)), - vector(x.size() - 1, VectorXd::Zero(0)), Q, - vector(x.size() - 1, MatrixXd::Zero(0, 0)), - vector(x.size() - 1, MatrixXd::Zero(0, 0))); -} - -double TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( - const vector& x, const vector& u, - const vector& lambda, const vector& x_des, - const vector& u_des, const vector& lambda_des, - const MatrixXd& Q, const MatrixXd& R, const MatrixXd& S) { - return ComputeQuadraticTrajectoryCost( - x, u, lambda, x_des, u_des, lambda_des, vector(x.size(), Q), - vector(u.size(), R), vector(lambda.size(), S)); -} - -double TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( - const vector& x, const vector& u, - const vector& x_des, const vector& u_des, - const MatrixXd& Q, const MatrixXd& R) { - return ComputeQuadraticTrajectoryCost(x, u, x_des, u_des, - vector(x.size(), Q), - vector(u.size(), R)); -} - -double TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( - const vector& x, const vector& x_des, - const MatrixXd& Q) { - return ComputeQuadraticTrajectoryCost(x, x_des, - vector(x.size(), Q)); -} - std::pair, vector> TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, const vector& u_plan, diff --git a/core/traj_eval.h b/core/traj_eval.h index bd0273b9..7af127b8 100644 --- a/core/traj_eval.h +++ b/core/traj_eval.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -7,6 +8,8 @@ #include "c3_options.h" #include "lcs.h" +#include "drake/common/drake_throw.h" + namespace c3 { namespace traj_eval { @@ -14,73 +17,61 @@ namespace traj_eval { class TrajectoryEvaluator { public: /** - * @brief Computes the quadratic cost of a trajectory of states, inputs, and - * contact forces, with respect to a desired trajectory and quadratic cost - * matrices. + * @brief Computes the quadratic cost of a trajectory of vectors with respect + * to a desired trajectory and quadratic cost matrices. Can sum multiple + * costs together by passing in multiple trajectories, desired trajectories, + * and cost matrices. The total cost is the sum of the individual costs, where + * each individual cost is computed as: * * Cost = Sum_{i = 0, ... N-1} - * (x_i-x_des_i)^T Q_i (x_i-x_des_i) + - * (u_i-u_des_i)^T R_i (u_i-u_des_i) + - * (lambda_i-lambda_des_i)^T S_i (lambda_i-lambda_des_i) - * + (x_N-x_des_N)^T Q_N (x_N-x_des_N) + * (data_i-data_des_i)^T CostMatrix_i (data_i-data_des_i) * - * @param x Trajectory of length N+1 of state vectors - * @param u Trajectory of length N of input vectors - * @param lambda Trajectory of length N of contact force vectors - * @param x_des Trajectory of length N+1 of desired state vectors - * @param u_des Trajectory of length N of desired input vectors - * @param lambda_des Trajectory of length N of desired contact force vectors - * @param Q Trajectory of length N+1 of state cost matrices - * @param R Trajectory of length N of input cost matrices - * @param S Trajectory of length N of contact force cost matrices + * @param data Trajectory of length N of vectors + * @param data_des Trajectory of length N of desired vectors + * @param cost_matrices Trajectory of length N of cost matrices. This can be + * a single cost matrix instead, in which case that cost + * matrix is used for all time steps. + * ... (optional additional trajectories, desired trajectories, and cost + * matrices, whose additional costs are summed together with the first) * @return The cost */ + template static double ComputeQuadraticTrajectoryCost( - const std::vector& x, - const std::vector& u, - const std::vector& lambda, - const std::vector& x_des, - const std::vector& u_des, - const std::vector& lambda_des, - const std::vector& Q, - const std::vector& R, - const std::vector& S); - /** @brief Special case: cost does not depend on the contact forces. */ - static double ComputeQuadraticTrajectoryCost( - const std::vector& x, - const std::vector& u, - const std::vector& x_des, - const std::vector& u_des, - const std::vector& Q, - const std::vector& R); - /** @brief Special case: cost does not depend on the inputs or contact forces. + const std::vector& data, + const std::vector& data_des, + const std::vector& cost_matrices, Rest&&... rest) { + DRAKE_THROW_UNLESS(data.size() == data_des.size()); + DRAKE_THROW_UNLESS(data.size() == cost_matrices.size()); + + int n_data = data[0].size(); + + double cost = 0.0; + for (size_t i = 0; i < data.size(); i++) { + DRAKE_THROW_UNLESS(data[i].size() == n_data); + DRAKE_THROW_UNLESS(data_des[i].size() == n_data); + DRAKE_THROW_UNLESS(cost_matrices[i].rows() == n_data); + DRAKE_THROW_UNLESS(cost_matrices[i].cols() == n_data); + + cost += (data[i] - data_des[i]).transpose() * cost_matrices[i] * + (data[i] - data_des[i]); + } + + return cost + ComputeQuadraticTrajectoryCost(std::forward(rest)...); + } + /** Special case: the same cost matrix is to be used throughout the trajectory + * so only one is provided. */ + template static double ComputeQuadraticTrajectoryCost( - const std::vector& x, - const std::vector& x_des, - const std::vector& Q); - /** @brief Special case: cost matrices are the same across all time steps. */ - static double ComputeQuadraticTrajectoryCost( - const std::vector& x, - const std::vector& u, - const std::vector& lambda, - const std::vector& x_des, - const std::vector& u_des, - const std::vector& lambda_des, const Eigen::MatrixXd& Q, - const Eigen::MatrixXd& R, const Eigen::MatrixXd& S); - /** @brief Special case: cost does not depend on the contact forces, and cost - * matrices are the same across all time steps. */ - static double ComputeQuadraticTrajectoryCost( - const std::vector& x, - const std::vector& u, - const std::vector& x_des, - const std::vector& u_des, const Eigen::MatrixXd& Q, - const Eigen::MatrixXd& R); - /** @brief Special case: cost does not depend on the inputs or contact forces, - * and cost matrices are the same across all time steps. */ - static double ComputeQuadraticTrajectoryCost( - const std::vector& x, - const std::vector& x_des, const Eigen::MatrixXd& Q); + const std::vector& data, + const std::vector& data_des, + const Eigen::MatrixXd& cost_matrix, Rest&&... rest) { + return ComputeQuadraticTrajectoryCost( + data, data_des, std::vector(data.size(), cost_matrix), + std::forward(rest)...); + } + /** Special case: no trajectory to evaluate. */ + static double ComputeQuadraticTrajectoryCost() { return 0.0; } /** * @brief From a planned trajectory of states and inputs and sets of Kp and Kd From 277ad7c6d4f9cb5d90917985f31cb19022cabc9f Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Wed, 18 Mar 2026 20:17:06 -0400 Subject: [PATCH 08/17] Incorporate reviewer feedback: more trajectory cost input argument options, corresponding tests, general clean up --- core/c3.h | 8 ++- core/test/core_test.cc | 91 +++++++++++++++++++++------ core/traj_eval.cc | 79 +++++++++++------------ core/traj_eval.h | 140 ++++++++++++++++++++++++++++++++++------- 4 files changed, 236 insertions(+), 82 deletions(-) diff --git a/core/c3.h b/core/c3.h index 498a8438..fdae5028 100644 --- a/core/c3.h +++ b/core/c3.h @@ -50,8 +50,6 @@ class C3 { std::vector R; std::vector G; std::vector U; - std::vector Q_evaluation; - std::vector R_evaluation; }; /*! @@ -197,6 +195,12 @@ class C3 { std::vector GetInputSolution() { return *u_sol_; } std::vector GetDualDeltaSolution() { return *delta_sol_; } std::vector GetDualWSolution() { return *w_sol_; } + std::vector GetDesiredState() { return x_desired_; } + const LCS& GetLCS() const { return lcs_; } + + bool GetPenalizeInputChange() const { + return options_.penalize_input_change.value_or(true); + } int GetZSize() const { return n_z_; } diff --git a/core/test/core_test.cc b/core/test/core_test.cc index 5bfda397..73b27330 100644 --- a/core/test/core_test.cc +++ b/core/test/core_test.cc @@ -361,6 +361,10 @@ TEST_F(TrajectoryEvaluatorTest, QuadraticCostMatchesManual) { actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(x, x_des, Q, u, u_des, R); EXPECT_NEAR(actual, expected, 1e-12); // State and input costs + actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(x, x_des, Q, u, + u_des[0], R); + EXPECT_NEAR(actual, expected, + 1e-12); // State and input costs with repeated desired input expected += 5.0 * 0.25 * 0.25; actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( @@ -378,12 +382,24 @@ TEST_F(TrajectoryEvaluatorTest, QuadraticCostMatchesManual) { actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(x, x_des, Q[0], u, u_des, R[0]); EXPECT_NEAR(actual, expected, 1e-12); // State and input costs + actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + x, x_des, Q[0], u, u_des[0], R[0]); + EXPECT_NEAR(actual, expected, + 1e-12); // State and input costs with repeated desired input expected += 5.0 * 0.25 * 0.25; actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( x, x_des, Q[0], u, u_des, R[0], lambda, lambda_des, S[0]); EXPECT_NEAR(actual, expected, 1e-12); // State, input, force costs + // Test with no desired trajectory (so target is assumed to be origin) + expected = 0.0; + expected += 2.0 * 1.0 * 1.0; + expected += 2.0 * 2.0 * 2.0; + actual = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(x, Q[0]); + EXPECT_NEAR(actual, expected, + 1e-12); // State costs with no desired trajectory + // Test any mismatched dimensions throw errors. std::vector x_wrong(3, VectorXd::Zero(1)); ASSERT_ANY_THROW( @@ -498,17 +514,17 @@ TEST_F(TrajectoryEvaluatorTest, ZeroOrderHoldAndDownsampleRoundTrip) { coarse[0] << 1.0, 2.0; coarse[1] << 3.0, 4.0; - const int timestep_split = 3; + const int upsample_rate = 3; std::vector fine = - TrajectoryEvaluator::ZeroOrderHoldTrajectory(coarse, timestep_split); - EXPECT_EQ(fine.size(), coarse.size() * timestep_split); - for (int i = 0; i < timestep_split; ++i) { + TrajectoryEvaluator::ZeroOrderHoldTrajectory(coarse, upsample_rate); + EXPECT_EQ(fine.size(), coarse.size() * upsample_rate); + for (int i = 0; i < upsample_rate; ++i) { EXPECT_TRUE(fine[i].isApprox(coarse[0])); - EXPECT_TRUE(fine[i + timestep_split].isApprox(coarse[1])); + EXPECT_TRUE(fine[i + upsample_rate].isApprox(coarse[1])); } std::vector downsampled = - TrajectoryEvaluator::DownsampleTrajectory(fine, timestep_split); + TrajectoryEvaluator::DownsampleTrajectory(fine, upsample_rate); EXPECT_EQ(downsampled.size(), coarse.size()); EXPECT_TRUE(downsampled[0].isApprox(coarse[0])); EXPECT_TRUE(downsampled[1].isApprox(coarse[1])); @@ -523,25 +539,25 @@ TEST_F(TrajectoryEvaluatorTest, MultiZeroOrderHoldAndDownsampleRoundTrip) { u_coarse[0] << 7.0; u_coarse[1] << 8.0; - const int timestep_split = 3; + const int upsample_rate = 3; std::pair, std::vector> fine = TrajectoryEvaluator::ZeroOrderHoldTrajectories(x_coarse, u_coarse, - timestep_split); + upsample_rate); std::vector x_fine = fine.first; std::vector u_fine = fine.second; - EXPECT_EQ(x_fine.size(), (x_coarse.size() - 1) * timestep_split + 1); - EXPECT_EQ(u_fine.size(), u_coarse.size() * timestep_split); - for (int i = 0; i < timestep_split; ++i) { + EXPECT_EQ(x_fine.size(), (x_coarse.size() - 1) * upsample_rate + 1); + EXPECT_EQ(u_fine.size(), u_coarse.size() * upsample_rate); + for (int i = 0; i < upsample_rate; ++i) { EXPECT_TRUE(x_fine[i].isApprox(x_coarse[0])); - EXPECT_TRUE(x_fine[i + timestep_split].isApprox(x_coarse[1])); + EXPECT_TRUE(x_fine[i + upsample_rate].isApprox(x_coarse[1])); EXPECT_TRUE(u_fine[i].isApprox(u_coarse[0])); - EXPECT_TRUE(u_fine[i + timestep_split].isApprox(u_coarse[1])); + EXPECT_TRUE(u_fine[i + upsample_rate].isApprox(u_coarse[1])); } EXPECT_TRUE(x_fine.back().isApprox(x_coarse.back())); std::pair, std::vector> downsampled = TrajectoryEvaluator::DownsampleTrajectories(x_fine, u_fine, - timestep_split); + upsample_rate); std::vector x_downsampled = downsampled.first; std::vector u_downsampled = downsampled.second; EXPECT_EQ(x_downsampled.size(), 3); @@ -557,16 +573,16 @@ TEST_F(TrajectoryEvaluatorTest, MultiZeroOrderHoldAndDownsampleRoundTrip) { // Test any mismatched dimensions throw errors. std::vector x_coarse_wrong(2, VectorXd::Zero(3)); ASSERT_ANY_THROW(TrajectoryEvaluator::ZeroOrderHoldTrajectories( - x_coarse_wrong, u_coarse, timestep_split)); + x_coarse_wrong, u_coarse, upsample_rate)); std::vector u_coarse_wrong(3, VectorXd::Zero(2)); ASSERT_ANY_THROW(TrajectoryEvaluator::ZeroOrderHoldTrajectories( - x_coarse, u_coarse_wrong, timestep_split)); + x_coarse, u_coarse_wrong, upsample_rate)); std::vector x_fine_wrong(2, VectorXd::Zero(3)); ASSERT_ANY_THROW(TrajectoryEvaluator::DownsampleTrajectories( - x_fine_wrong, u_fine, timestep_split)); + x_fine_wrong, u_fine, upsample_rate)); std::vector u_fine_wrong(3, VectorXd::Zero(2)); ASSERT_ANY_THROW(TrajectoryEvaluator::DownsampleTrajectories( - x_fine, u_fine_wrong, timestep_split)); + x_fine, u_fine_wrong, upsample_rate)); } TEST_F(TrajectoryEvaluatorTest, @@ -707,6 +723,45 @@ TYPED_TEST(C3CartpoleTypedTest, End2EndCartpoleTest) { ASSERT_EQ(x[timesteps - 1].isZero(0.1), true); } +TYPED_TEST(C3CartpoleTypedTest, ComputeCost) { + C3Options options_no_input_change = this->options; + options_no_input_change.penalize_input_change = false; + TypeParam optimizer(*this->pSystem, this->cost, this->xdesired, + options_no_input_change); + + // Solve one iteration of the problem. + optimizer.Solve(this->x0); + std::vector x_sol = optimizer.GetStateSolution(); + std::vector u_sol = optimizer.GetInputSolution(); + std::vector lambda_sol = optimizer.GetForceSolution(); + + // The state trajectory excludes the N+1 time step. Add it to the end via + // LCS rollout since it contributes to the C3 internal cost optimization. + const LCS& lcs = optimizer.GetLCS(); + const MatrixXd& A_N = lcs.A().back(); + const MatrixXd& B_N = lcs.B().back(); + const MatrixXd& D_N = lcs.D().back(); + const VectorXd& d_N = lcs.d().back(); + x_sol.push_back(A_N * x_sol.back() + B_N * u_sol.back() + + D_N * lambda_sol.back() + d_N); + + // Get the cost matrices and desired state. + const C3::CostMatrices cost_matrices = optimizer.GetCostMatrices(); + std::vector Q = cost_matrices.Q; + std::vector R = cost_matrices.R; + std::vector x_des = optimizer.GetDesiredState(); + ASSERT_EQ(x_sol.size(), x_des.size()); + ASSERT_EQ(x_sol.size(), Q.size()); + ASSERT_EQ(u_sol.size(), R.size()); + + // Compute the cost using the TrajectoryEvaluator + double cost_from_c3_object = + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(&optimizer); + double cost_from_traj = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + x_sol, x_des, Q, u_sol, R); + EXPECT_NEAR(cost_from_c3_object, cost_from_traj, 1e-12); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/core/traj_eval.cc b/core/traj_eval.cc index afeddc3f..b07a1374 100644 --- a/core/traj_eval.cc +++ b/core/traj_eval.cc @@ -68,33 +68,28 @@ TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, const VectorXd& Kd, const LCS& coarse_lcs, const LCS& fine_lcs) { - int timestep_split = CheckCoarseAndFineLCSCompatibility(coarse_lcs, fine_lcs); + int upsample_rate = CheckCoarseAndFineLCSCompatibility(coarse_lcs, fine_lcs); // Zero-order hold the planned trajectory to match the finer time // discretization of the LCS. - std::pair, vector> fine_plans = - ZeroOrderHoldTrajectories(x_plan, u_plan, timestep_split); - vector x_plan_finer = fine_plans.first; - vector u_plan_finer = fine_plans.second; + auto [x_plan_finer, u_plan_finer] = + ZeroOrderHoldTrajectories(x_plan, u_plan, upsample_rate); // Do PD control with the finer trajectory and LCS. - std::pair, vector> fine_sims = + auto [x_sim_fine, u_sim_fine] = SimulatePDControlWithLCS(x_plan_finer, u_plan_finer, Kp, Kd, fine_lcs); - vector x_sim_fine = fine_sims.first; - vector u_sim_fine = fine_sims.second; // Downsample the resulting trajectory back to the original time // discretization. - std::pair, vector> downsampled_sims = - DownsampleTrajectories(x_sim_fine, u_sim_fine, timestep_split); - vector x_sim = downsampled_sims.first; - vector u_sim = downsampled_sims.second; + auto [x_sim, u_sim] = + DownsampleTrajectories(x_sim_fine, u_sim_fine, upsample_rate); return std::make_pair(x_sim, u_sim); } vector TrajectoryEvaluator::SimulateLCSOverTrajectory( - const VectorXd& x_init, const vector& u_plan, const LCS& lcs) { + const VectorXd& x_init, const vector& u_plan, const LCS& lcs, + const LCSSimulateConfig& config) { int N = lcs.N(); CheckLCSAndTrajectoryCompatibility(lcs, vector(N + 1, x_init), u_plan); @@ -103,46 +98,49 @@ vector TrajectoryEvaluator::SimulateLCSOverTrajectory( vector(N + 1, VectorXd::Zero(x_init.size())); x_sim[0] = x_init; for (int i = 0; i < N; i++) { - x_sim[i + 1] = lcs.Simulate(x_sim[i], u_plan[i]); + x_sim[i + 1] = lcs.Simulate(x_sim[i], u_plan[i], config); } return x_sim; } vector TrajectoryEvaluator::SimulateLCSOverTrajectory( const vector& x_plan, const vector& u_plan, - const LCS& lcs) { - return SimulateLCSOverTrajectory(x_plan[0], u_plan, lcs); + const LCS& lcs, const LCSSimulateConfig& config) { + return SimulateLCSOverTrajectory(x_plan[0], u_plan, lcs, config); } vector TrajectoryEvaluator::SimulateLCSOverTrajectory( const VectorXd& x_init, const vector& u_plan, - const LCS& coarse_lcs, const LCS& fine_lcs) { - int timestep_split = CheckCoarseAndFineLCSCompatibility(coarse_lcs, fine_lcs); + const LCS& coarse_lcs, const LCS& fine_lcs, + const LCSSimulateConfig& config) { + int upsample_rate = CheckCoarseAndFineLCSCompatibility(coarse_lcs, fine_lcs); vector u_plan_finer = - ZeroOrderHoldTrajectory(u_plan, timestep_split); + ZeroOrderHoldTrajectory(u_plan, upsample_rate); vector x_sim_fine = - SimulateLCSOverTrajectory(x_init, u_plan_finer, fine_lcs); - return DownsampleTrajectories(x_sim_fine, u_plan_finer, timestep_split).first; + SimulateLCSOverTrajectory(x_init, u_plan_finer, fine_lcs, config); + return DownsampleTrajectories(x_sim_fine, u_plan_finer, upsample_rate).first; } vector TrajectoryEvaluator::SimulateLCSOverTrajectory( const vector& x_plan, const vector& u_plan, - const LCS& coarse_lcs, const LCS& fine_lcs) { - return SimulateLCSOverTrajectory(x_plan[0], u_plan, coarse_lcs, fine_lcs); + const LCS& coarse_lcs, const LCS& fine_lcs, + const LCSSimulateConfig& config) { + return SimulateLCSOverTrajectory(x_plan[0], u_plan, coarse_lcs, fine_lcs, + config); } vector TrajectoryEvaluator::ZeroOrderHoldTrajectory( - const vector& coarse_traj, const int& timestep_split) { + const vector& coarse_traj, const int& upsample_rate) { int N = coarse_traj.size(); // Zero-order hold the planned trajectory to match the finer time // discretization of the LCS. - vector fine_traj(N * timestep_split, + vector fine_traj(N * upsample_rate, VectorXd::Zero(coarse_traj[0].size())); for (int i = 0; i < N; i++) { - for (int j = 0; j < timestep_split; j++) { - fine_traj[i * timestep_split + j] = coarse_traj[i]; + for (int j = 0; j < upsample_rate; j++) { + fine_traj[i * upsample_rate + j] = coarse_traj[i]; } } return fine_traj; @@ -151,24 +149,24 @@ vector TrajectoryEvaluator::ZeroOrderHoldTrajectory( std::pair, vector> TrajectoryEvaluator::ZeroOrderHoldTrajectories(const vector& x_coarse, const vector& u_coarse, - const int& timestep_split) { + const int& upsample_rate) { DRAKE_THROW_UNLESS(x_coarse.size() == u_coarse.size() + 1); vector x_fine = ZeroOrderHoldTrajectory( - vector(x_coarse.begin(), x_coarse.end() - 1), timestep_split); + vector(x_coarse.begin(), x_coarse.end() - 1), upsample_rate); x_fine.push_back(x_coarse.back()); - vector u_fine = ZeroOrderHoldTrajectory(u_coarse, timestep_split); + vector u_fine = ZeroOrderHoldTrajectory(u_coarse, upsample_rate); return std::make_pair(x_fine, u_fine); } vector TrajectoryEvaluator::DownsampleTrajectory( - const vector& fine_traj, const int& timestep_split) { - DRAKE_THROW_UNLESS(fine_traj.size() % timestep_split == 0); - int N = fine_traj.size() / timestep_split; + const vector& fine_traj, const int& upsample_rate) { + DRAKE_THROW_UNLESS(fine_traj.size() % upsample_rate == 0); + int N = fine_traj.size() / upsample_rate; // Downsample the fine trajectory. vector coarse_traj(N, VectorXd::Zero(fine_traj[0].size())); for (int i = 0; i < N; i++) { - coarse_traj[i] = fine_traj[i * timestep_split]; + coarse_traj[i] = fine_traj[i * upsample_rate]; } return coarse_traj; } @@ -176,12 +174,12 @@ vector TrajectoryEvaluator::DownsampleTrajectory( std::pair, vector> TrajectoryEvaluator::DownsampleTrajectories(const vector& x_fine, const vector& u_fine, - const int& timestep_split) { + const int& upsample_rate) { DRAKE_THROW_UNLESS(x_fine.size() == u_fine.size() + 1); vector x_coarse = DownsampleTrajectory( - vector(x_fine.begin(), x_fine.end() - 1), timestep_split); + vector(x_fine.begin(), x_fine.end() - 1), upsample_rate); x_coarse.push_back(x_fine.back()); - vector u_coarse = DownsampleTrajectory(u_fine, timestep_split); + vector u_coarse = DownsampleTrajectory(u_fine, upsample_rate); return std::make_pair(x_coarse, u_coarse); } @@ -193,10 +191,9 @@ int TrajectoryEvaluator::CheckCoarseAndFineLCSCompatibility( DRAKE_THROW_UNLESS(coarse_lcs.dt() >= fine_lcs.dt()); DRAKE_THROW_UNLESS(coarse_lcs.N() * coarse_lcs.dt() == fine_lcs.N() * fine_lcs.dt()); - int timestep_split = fine_lcs.N() / coarse_lcs.N(); - DRAKE_THROW_UNLESS(fine_lcs.dt() * timestep_split == coarse_lcs.dt()); - DRAKE_THROW_UNLESS(coarse_lcs.N() * timestep_split == fine_lcs.N()); - return timestep_split; + int upsample_rate = fine_lcs.N() / coarse_lcs.N(); + DRAKE_THROW_UNLESS(fine_lcs.dt() * upsample_rate == coarse_lcs.dt()); + return upsample_rate; } void TrajectoryEvaluator::CheckLCSAndTrajectoryCompatibility( diff --git a/core/traj_eval.h b/core/traj_eval.h index 7af127b8..ef8eb431 100644 --- a/core/traj_eval.h +++ b/core/traj_eval.h @@ -5,6 +5,7 @@ #include +#include "c3.h" #include "c3_options.h" #include "lcs.h" @@ -58,8 +59,8 @@ class TrajectoryEvaluator { return cost + ComputeQuadraticTrajectoryCost(std::forward(rest)...); } - /** Special case: the same cost matrix is to be used throughout the trajectory - * so only one is provided. + /** @brief Special case: the same cost matrix is to be used throughout the + * trajectory so only one is provided. */ template static double ComputeQuadraticTrajectoryCost( @@ -70,8 +71,94 @@ class TrajectoryEvaluator { data, data_des, std::vector(data.size(), cost_matrix), std::forward(rest)...); } - /** Special case: no trajectory to evaluate. */ - static double ComputeQuadraticTrajectoryCost() { return 0.0; } + /** @brief Special case: the same desired data is to be used throughout the + * trajectory so only one is provided. + */ + template + static double ComputeQuadraticTrajectoryCost( + const std::vector& data, const Eigen::VectorXd& data_des, + const std::vector& cost_matrices, Rest&&... rest) { + return ComputeQuadraticTrajectoryCost( + data, std::vector(data.size(), data_des), + cost_matrices, std::forward(rest)...); + } + /** @brief Special case: the same desired data and the same cost matrix is to + * be used throughout the trajectory so only one each is provided. + */ + template + static double ComputeQuadraticTrajectoryCost( + const std::vector& data, const Eigen::VectorXd& data_des, + const Eigen::MatrixXd& cost_matrix, Rest&&... rest) { + return ComputeQuadraticTrajectoryCost( + data, std::vector(data.size(), data_des), + std::vector(data.size(), cost_matrix), + std::forward(rest)...); + } + /** @brief Special case: no desired data is provided, so it is assumed the + * desired data is zero. + */ + template + static double ComputeQuadraticTrajectoryCost( + const std::vector& data, + const std::vector& cost_matrices, Rest&&... rest) { + return ComputeQuadraticTrajectoryCost( + data, + std::vector(data.size(), + Eigen::VectorXd::Zero(data[0].size())), + cost_matrices, std::forward(rest)...); + } + /** @brief Special case: no desired data is provided and the same cost matrix + * is to be used throughout the trajectory, so it is assumed the desired data + * is zero and only one cost matrix is provided. + */ + template + static double ComputeQuadraticTrajectoryCost( + const std::vector& data, + const Eigen::MatrixXd& cost_matrix, Rest&&... rest) { + return ComputeQuadraticTrajectoryCost( + data, + std::vector(data.size(), + Eigen::VectorXd::Zero(data[0].size())), + std::vector(data.size(), cost_matrix), + std::forward(rest)...); + } + /** @brief Special case: a C3 object is the input argument, and the cost is to + * be computed based on the trajectories contained in the C3 object and the + * cost matrices contained in the C3 object for state and input costs. + * + * NOTE: This can only handle u^T R u input costs, not (u-u_prev)^T R + * (u-u_prev) input costs, since the C3 object does not contain u_prev + * trajectories. If the C3 object is configured to compute input costs based + * on (u-u_prev), this function throws an error. + */ + static double ComputeQuadraticTrajectoryCost(C3* c3) { + DRAKE_THROW_UNLESS(c3->GetPenalizeInputChange() == false); + + // Get the trajectories (solved for state and input, and desired for state). + std::vector x = c3->GetStateSolution(); + std::vector u = c3->GetInputSolution(); + std::vector lambda = c3->GetForceSolution(); + std::vector x_des = c3->GetDesiredState(); + + // The state trajectory excludes the N+1 time step. Add it to the end via + // LCS rollout since it contributes to the C3 internal cost optimization. + const LCS& lcs = c3->GetLCS(); + Eigen::MatrixXd A_N = lcs.A().back(); + Eigen::MatrixXd B_N = lcs.B().back(); + Eigen::MatrixXd D_N = lcs.D().back(); + Eigen::VectorXd d_N = lcs.d().back(); + x.push_back(A_N * x.back() + B_N * u.back() + D_N * lambda.back() + d_N); + + // Get the cost matrices. + std::vector Q = c3->GetCostMatrices().Q; + std::vector R = c3->GetCostMatrices().R; + + DRAKE_THROW_UNLESS(x_des.size() == x.size()); + DRAKE_THROW_UNLESS(Q.size() == x.size()); + DRAKE_THROW_UNLESS(R.size() == u.size()); + + return ComputeQuadraticTrajectoryCost(x, x_des, Q, u, R); + } /** * @brief From a planned trajectory of states and inputs and sets of Kp and Kd @@ -110,16 +197,18 @@ class TrajectoryEvaluator { * @param x_init Initial state * @param u_plan Trajectory of inputs (length N) * @param lcs LCS system to simulate + * @param config Configuration for simulating the LCS * @return Trajectory of simulated states (length N+1) */ static std::vector SimulateLCSOverTrajectory( const Eigen::VectorXd& x_init, const std::vector& u_plan, - const LCS& lcs); + const LCS& lcs, const LCSSimulateConfig& config = LCSSimulateConfig()); /** @brief Special case: a full trajectory of states is provided, but only the * initial state is used for simulation. */ static std::vector SimulateLCSOverTrajectory( const std::vector& x_plan, - const std::vector& u_plan, const LCS& lcs); + const std::vector& u_plan, const LCS& lcs, + const LCSSimulateConfig& config = LCSSimulateConfig()); /** * @brief Special case: simulate plans from a coarser LCS with a finer LCS. * The returned trajectory is downsampled back to be compatible with the @@ -128,7 +217,8 @@ class TrajectoryEvaluator { */ static std::vector SimulateLCSOverTrajectory( const Eigen::VectorXd& x_init, const std::vector& u_plan, - const LCS& coarse_lcs, const LCS& fine_lcs); + const LCS& coarse_lcs, const LCS& fine_lcs, + const LCSSimulateConfig& config = LCSSimulateConfig()); /** * @brief Special case: simulate a trajectory from a coarser LCS with a finer * LCS, where a full trajectory of states is provided but only the initial @@ -137,7 +227,8 @@ class TrajectoryEvaluator { static std::vector SimulateLCSOverTrajectory( const std::vector& x_plan, const std::vector& u_plan, const LCS& coarse_lcs, - const LCS& fine_lcs); + const LCS& fine_lcs, + const LCSSimulateConfig& config = LCSSimulateConfig()); /** * @brief Zero order hold a trajectory compatible with one LCS to be @@ -145,26 +236,26 @@ class TrajectoryEvaluator { * must be compatible with each other, i.e. the finer horizon must be an * integer multiple of the coarser horizon, and dt*N must be the same for both * LCSs. The state trajectory (which was N+1 in length) becomes - * (N*timestep_split)+1 in length, and the input trajectory (which was N in - * length) becomes N*timestep_split in length. + * (N*upsample_rate)+1 in length, and the input trajectory (which was N in + * length) becomes N*upsample_rate in length. * * @param x_coarse Coarse state trajectory (length N+1) * @param u_coarse Coarse input trajectory (length N) - * @param timestep_split Integer multiple for time discretization - * @return Pair of 1) fine state trajectory (length (N*timestep_split)+1) and - * 2) fine input trajectory (length N*timestep_split) + * @param upsample_rate Integer multiple for time discretization + * @return Pair of 1) fine state trajectory (length (N*upsample_rate)+1) and + * 2) fine input trajectory (length N*upsample_rate) */ static std::pair, std::vector> ZeroOrderHoldTrajectories(const std::vector& x_coarse, const std::vector& u_coarse, - const int& timestep_split); + const int& upsample_rate); /** * @brief Same as above but only one trajectory is provided and processed. - * Trajectories of length N become trajectories of length N*timestep_split. + * Trajectories of length N become trajectories of length N*upsample_rate. */ static std::vector ZeroOrderHoldTrajectory( const std::vector& coarse_traj, - const int& timestep_split); + const int& upsample_rate); /** * @brief The reverse of ZeroOrderHoldTrajectoryToFinerLCS: downsample a @@ -172,19 +263,19 @@ class TrajectoryEvaluator { * The same compatibility requirements apply as for * ZeroOrderHoldTrajectoryToFinerLCS. * - * @param x_fine Fine state trajectory (length (N*timestep_split)+1) - * @param u_fine Fine input trajectory (length N*timestep_split) - * @param timestep_split Integer multiple for time discretization + * @param x_fine Fine state trajectory (length (N*upsample_rate)+1) + * @param u_fine Fine input trajectory (length N*upsample_rate) + * @param upsample_rate Integer multiple for time discretization * @return Pair of 1) coarse state trajectory (length N+1) and 2) coarse input * trajectory (length N) */ static std::pair, std::vector> DownsampleTrajectories(const std::vector& x_fine, const std::vector& u_fine, - const int& timestep_split); + const int& upsample_rate); /** @brief Same as above but only one trajectory is provided and processed. */ static std::vector DownsampleTrajectory( - const std::vector& fine_traj, const int& timestep_split); + const std::vector& fine_traj, const int& upsample_rate); /** * @brief Check that two LCSs are compatible with each other, i.e. that the @@ -226,6 +317,13 @@ class TrajectoryEvaluator { /** @brief Special case: no inputs or lambdas provided. */ static void CheckLCSAndTrajectoryCompatibility( const LCS& lcs, const std::vector& x); + + private: + /** @brief Special case of ComputeQuadraticTrajectoryCost: no trajectory to + * evaluate. Required for variadic template recursion base case, but should + * not be called externally with no input arguments. + */ + static double ComputeQuadraticTrajectoryCost() { return 0.0; } }; } // namespace traj_eval From 25f0d4a2daf44536fbf353f864771b51033a3586 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Tue, 24 Mar 2026 13:34:50 -0400 Subject: [PATCH 09/17] Add cost calculation where trajectory from C3 object is paired with different cost matrices than those from C3 object; address minor PR comments --- core/test/core_test.cc | 12 ++++++++ core/traj_eval.h | 67 ++++++++++++++++++++++++++++++++++-------- 2 files changed, 67 insertions(+), 12 deletions(-) diff --git a/core/test/core_test.cc b/core/test/core_test.cc index 73b27330..de6f4350 100644 --- a/core/test/core_test.cc +++ b/core/test/core_test.cc @@ -753,6 +753,7 @@ TYPED_TEST(C3CartpoleTypedTest, ComputeCost) { ASSERT_EQ(x_sol.size(), x_des.size()); ASSERT_EQ(x_sol.size(), Q.size()); ASSERT_EQ(u_sol.size(), R.size()); + ASSERT_EQ(x_sol.size(), u_sol.size() + 1); // Compute the cost using the TrajectoryEvaluator double cost_from_c3_object = @@ -760,6 +761,17 @@ TYPED_TEST(C3CartpoleTypedTest, ComputeCost) { double cost_from_traj = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( x_sol, x_des, Q, u_sol, R); EXPECT_NEAR(cost_from_c3_object, cost_from_traj, 1e-12); + + cost_from_c3_object = + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(&optimizer, Q, R); + EXPECT_NEAR(cost_from_c3_object, cost_from_traj, 1e-12); + + cost_from_c3_object = + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(&optimizer, Q, R[0]); + EXPECT_NEAR(cost_from_c3_object, cost_from_traj, 1e-12); + + // NOTE: Don't test the Q[0] version since this example has a time-varying + // cost matrix. } int main(int argc, char** argv) { diff --git a/core/traj_eval.h b/core/traj_eval.h index ef8eb431..311c4826 100644 --- a/core/traj_eval.h +++ b/core/traj_eval.h @@ -35,6 +35,13 @@ class TrajectoryEvaluator { * ... (optional additional trajectories, desired trajectories, and cost * matrices, whose additional costs are summed together with the first) * @return The cost + * + * NOTE: To have this arbitrary number of trajectories (implemented via the + * Rest input argument with variadic templates), the function definition needs + * to be in the header file, which is why this function is defined here + * instead of in the .cc file. To have source code in the .cc file instead, + * explicit instantiations of every argument pattern are needed, which is more + * verbose and less flexible than the variadic template approach. */ template static double ComputeQuadraticTrajectoryCost( @@ -132,6 +139,23 @@ class TrajectoryEvaluator { * on (u-u_prev), this function throws an error. */ static double ComputeQuadraticTrajectoryCost(C3* c3) { + // Get the cost matrices. + std::vector Q = c3->GetCostMatrices().Q; + std::vector R = c3->GetCostMatrices().R; + + return ComputeQuadraticTrajectoryCost(c3, Q, R); + } + /** @brief Special case: a C3 object and different cost matrices are the input + * arguments. + * + * NOTE: This can only handle u^T R u input costs, not (u-u_prev)^T R + * (u-u_prev) input costs, since the C3 object does not contain u_prev + * trajectories. If the C3 object is configured to compute input costs based + * on (u-u_prev), this function throws an error. + */ + static double ComputeQuadraticTrajectoryCost( + C3* c3, const std::vector& Q, + const std::vector& R) { DRAKE_THROW_UNLESS(c3->GetPenalizeInputChange() == false); // Get the trajectories (solved for state and input, and desired for state). @@ -140,8 +164,9 @@ class TrajectoryEvaluator { std::vector lambda = c3->GetForceSolution(); std::vector x_des = c3->GetDesiredState(); - // The state trajectory excludes the N+1 time step. Add it to the end via - // LCS rollout since it contributes to the C3 internal cost optimization. + // The state trajectory from GetStateSolution excludes the N+1 time step. + // Add it to the end via LCS rollout since it contributes to the C3 internal + // cost optimization. const LCS& lcs = c3->GetLCS(); Eigen::MatrixXd A_N = lcs.A().back(); Eigen::MatrixXd B_N = lcs.B().back(); @@ -149,27 +174,45 @@ class TrajectoryEvaluator { Eigen::VectorXd d_N = lcs.d().back(); x.push_back(A_N * x.back() + B_N * u.back() + D_N * lambda.back() + d_N); - // Get the cost matrices. - std::vector Q = c3->GetCostMatrices().Q; - std::vector R = c3->GetCostMatrices().R; - DRAKE_THROW_UNLESS(x_des.size() == x.size()); + DRAKE_THROW_UNLESS(x.size() == u.size() + 1); DRAKE_THROW_UNLESS(Q.size() == x.size()); DRAKE_THROW_UNLESS(R.size() == u.size()); return ComputeQuadraticTrajectoryCost(x, x_des, Q, u, R); } + /** @brief Special cases (3x): a C3 object and singular cost matrices (either + * Q, or R, or both) are the input arguments. */ + static double ComputeQuadraticTrajectoryCost( + C3* c3, const Eigen::MatrixXd& Q, const std::vector& R) { + return ComputeQuadraticTrajectoryCost( + c3, std::vector(c3->GetStateSolution().size() + 1, Q), + R); + } + static double ComputeQuadraticTrajectoryCost( + C3* c3, const std::vector& Q, const Eigen::MatrixXd& R) { + return ComputeQuadraticTrajectoryCost( + c3, Q, std::vector(c3->GetInputSolution().size(), R)); + } + static double ComputeQuadraticTrajectoryCost(C3* c3, const Eigen::MatrixXd& Q, + const Eigen::MatrixXd& R) { + return ComputeQuadraticTrajectoryCost( + c3, std::vector(c3->GetStateSolution().size() + 1, Q), + std::vector(c3->GetInputSolution().size(), R)); + } /** - * @brief From a planned trajectory of states and inputs and sets of Kp and Kd - * gains, use an LCS to simulate tracking the plan with PD control (with - * feedforward control), and return the resulting trajectory of actual states - * and inputs. + * @brief From a planned trajectory of states and inputs and sets of Kp + * and Kd gains, use an LCS to simulate tracking the plan with PD control + * (with feedforward control), and return the resulting trajectory of + * actual states and inputs. * * @param x_plan Planned trajectory of states (length N+1) * @param u_plan Planned trajectory of inputs (length N) - * @param Kp Proportional gains (length n_x, with exactly k non-zero entries) - * @param Kd Derivative gains (length n_x, with exactly k non-zero entries) + * @param Kp Proportional gains (length n_x, with exactly k non-zero + * entries) + * @param Kd Derivative gains (length n_x, with exactly k non-zero + * entries) * @param lcs LCS system to simulate * @return Pair of (simulated states, simulated inputs) */ From 0752816ff11de481fd19a9f5011842a07bca31d1 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Thu, 26 Mar 2026 12:14:34 -0400 Subject: [PATCH 10/17] Add python bindings for trajectory evaluation functions --- bindings/pyc3/BUILD.bazel | 14 +++ bindings/pyc3/__init__.py | 1 + bindings/pyc3/traj_eval_py.cc | 219 ++++++++++++++++++++++++++++++++++ 3 files changed, 234 insertions(+) create mode 100644 bindings/pyc3/traj_eval_py.cc diff --git a/bindings/pyc3/BUILD.bazel b/bindings/pyc3/BUILD.bazel index ced08862..b48c7d8a 100644 --- a/bindings/pyc3/BUILD.bazel +++ b/bindings/pyc3/BUILD.bazel @@ -24,6 +24,19 @@ pybind_py_library( py_imports = ["."], ) +pybind_py_library( + name = "traj_eval_py", + cc_deps = ["//core:lcs", + "//core:traj_eval", + "//core:c3"], + cc_so_name = "traj_eval", + cc_srcs = ["traj_eval_py.cc"], + py_deps = [ + ":module_py", + ], + py_imports = ["."], +) + pybind_py_library( name = "c3_multibody_py", cc_deps = ["//multibody:lcs_factory", @@ -75,6 +88,7 @@ py_library( PY_LIBRARIES = [ ":module_py", ":c3_py", + ":traj_eval_py", ":c3_systems_py", ":c3_multibody_py", ] diff --git a/bindings/pyc3/__init__.py b/bindings/pyc3/__init__.py index 323a982c..df08b6d4 100644 --- a/bindings/pyc3/__init__.py +++ b/bindings/pyc3/__init__.py @@ -2,5 +2,6 @@ import pydrake from . import * from .c3 import * +from .traj_eval import * from .systems import * from .multibody import * diff --git a/bindings/pyc3/traj_eval_py.cc b/bindings/pyc3/traj_eval_py.cc new file mode 100644 index 00000000..b2bd7ecb --- /dev/null +++ b/bindings/pyc3/traj_eval_py.cc @@ -0,0 +1,219 @@ +#include +#include +#include +#include + +#include "core/lcs.h" +#include "core/traj_eval.h" + +namespace py = pybind11; + +namespace c3 { +namespace pyc3 { + +PYBIND11_MODULE(traj_eval, m) { + // NOTE: C++ API Flexibility vs. Python Bindings Limitation + // The C++ TrajectoryEvaluator::ComputeQuadraticTrajectoryCost() uses variadic + // templates to support arbitrary input combinations (e.g., + // ComputeQuadraticTrajectoryCost(x, x_des, Q, u, R)). However, Python's + // pybind11 does not support variadic template forwarding natively. Therefore, + // the Python bindings only expose a finite set of predefined overloads. If + // additional input combinations are needed for Python use cases, they must be + // explicitly added to this file with corresponding static_cast declarations. + + // LCSSimulateConfig struct bindings + py::class_(m, "LCSSimulateConfig") + .def(py::init<>()) + .def_readwrite("regularized", &LCSSimulateConfig::regularized) + .def_readwrite("piv_tol", &LCSSimulateConfig::piv_tol) + .def_readwrite("zero_tol", &LCSSimulateConfig::zero_tol) + .def_readwrite("min_exp", &LCSSimulateConfig::min_exp) + .def_readwrite("step_exp", &LCSSimulateConfig::step_exp) + .def_readwrite("max_exp", &LCSSimulateConfig::max_exp); + + // TrajectoryEvaluator class bindings + py::class_(m, "TrajectoryEvaluator") + + // ComputeQuadraticTrajectoryCost overloads + .def_static( + "ComputeQuadraticTrajectoryCost", + static_cast&, + const std::vector&, + const std::vector&)>( + &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + py::arg("data"), py::arg("data_des"), py::arg("cost_matrices"), + "Compute quadratic cost for full trajectory cost computation") + .def_static( + "ComputeQuadraticTrajectoryCost", + static_cast&, + const std::vector&, + const Eigen::MatrixXd&)>( + &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + py::arg("data"), py::arg("data_des"), py::arg("cost_matrix"), + "Compute quadratic cost with single cost matrix") + .def_static( + "ComputeQuadraticTrajectoryCost", + static_cast&, + const std::vector&)>( + &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + py::arg("data"), py::arg("cost_matrices"), + "Compute quadratic cost assuming zero desired data") + .def_static( + "ComputeQuadraticTrajectoryCost", + static_cast&, + const Eigen::MatrixXd&)>( + &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + py::arg("data"), py::arg("cost_matrix"), + "Compute quadratic cost with single matrix and zero desired") + .def_static( + "ComputeQuadraticTrajectoryCost", + static_cast( + &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + py::arg("c3"), "Compute cost from C3 optimizer solution") + .def_static( + "ComputeQuadraticTrajectoryCost", + static_cast&, + const std::vector&)>( + &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + py::arg("c3"), py::arg("Q"), py::arg("R"), + "Compute cost from C3 with custom cost matrices") + .def_static( + "ComputeQuadraticTrajectoryCost", + static_cast&)>( + &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + py::arg("c3"), py::arg("Q"), py::arg("R"), + "Compute cost from C3 with single Q matrix") + .def_static( + "ComputeQuadraticTrajectoryCost", + static_cast&, + const Eigen::MatrixXd&)>( + &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + py::arg("c3"), py::arg("Q"), py::arg("R"), + "Compute cost from C3 with single R matrix") + .def_static( + "ComputeQuadraticTrajectoryCost", + static_cast( + &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + py::arg("c3"), py::arg("Q"), py::arg("R"), + "Compute cost from C3 with single Q and R matrices") + + // SimulatePDControlWithLCS overloads + .def_static( + "SimulatePDControlWithLCS", + static_cast, + std::vector> (*)( + const std::vector&, + const std::vector&, const Eigen::VectorXd&, + const Eigen::VectorXd&, const LCS&)>( + &traj_eval::TrajectoryEvaluator::SimulatePDControlWithLCS), + py::arg("x_plan"), py::arg("u_plan"), py::arg("Kp"), py::arg("Kd"), + py::arg("lcs"), "Simulate PD control with an LCS") + .def_static( + "SimulatePDControlWithLCS", + static_cast, + std::vector> (*)( + const std::vector&, + const std::vector&, const Eigen::VectorXd&, + const Eigen::VectorXd&, const LCS&, const LCS&)>( + &traj_eval::TrajectoryEvaluator::SimulatePDControlWithLCS), + py::arg("x_plan"), py::arg("u_plan"), py::arg("Kp"), py::arg("Kd"), + py::arg("coarse_lcs"), py::arg("fine_lcs"), + "Simulate PD control with coarse and fine LCSs") + + // SimulateLCSOverTrajectory overloads + .def_static( + "SimulateLCSOverTrajectory", + static_cast (*)( + const Eigen::VectorXd&, const std::vector&, + const LCS&, const c3::LCSSimulateConfig&)>( + &traj_eval::TrajectoryEvaluator::SimulateLCSOverTrajectory), + py::arg("x_init"), py::arg("u_plan"), py::arg("lcs"), + py::arg("config") = c3::LCSSimulateConfig(), + "Simulate LCS over input trajectory from initial state") + .def_static( + "SimulateLCSOverTrajectory", + static_cast (*)( + const std::vector&, + const std::vector&, const LCS&, + const c3::LCSSimulateConfig&)>( + &traj_eval::TrajectoryEvaluator::SimulateLCSOverTrajectory), + py::arg("x_plan"), py::arg("u_plan"), py::arg("lcs"), + py::arg("config") = c3::LCSSimulateConfig(), + "Simulate LCS over trajectory using initial state from plan") + .def_static( + "SimulateLCSOverTrajectory", + static_cast (*)( + const Eigen::VectorXd&, const std::vector&, + const LCS&, const LCS&, const c3::LCSSimulateConfig&)>( + &traj_eval::TrajectoryEvaluator::SimulateLCSOverTrajectory), + py::arg("x_init"), py::arg("u_plan"), py::arg("coarse_lcs"), + py::arg("fine_lcs"), py::arg("config") = c3::LCSSimulateConfig(), + "Simulate with coarse and fine LCSs from initial state") + .def_static( + "SimulateLCSOverTrajectory", + static_cast (*)( + const std::vector&, + const std::vector&, const LCS&, const LCS&, + const c3::LCSSimulateConfig&)>( + &traj_eval::TrajectoryEvaluator::SimulateLCSOverTrajectory), + py::arg("x_plan"), py::arg("u_plan"), py::arg("coarse_lcs"), + py::arg("fine_lcs"), py::arg("config") = c3::LCSSimulateConfig(), + "Simulate with coarse and fine LCSs using plan initial state") + + // ZeroOrderHold methods + .def_static("ZeroOrderHoldTrajectories", + &traj_eval::TrajectoryEvaluator::ZeroOrderHoldTrajectories, + py::arg("x_coarse"), py::arg("u_coarse"), + py::arg("upsample_rate"), + "Upsample state and input trajectories") + .def_static("ZeroOrderHoldTrajectory", + &traj_eval::TrajectoryEvaluator::ZeroOrderHoldTrajectory, + py::arg("coarse_traj"), py::arg("upsample_rate"), + "Upsample a single trajectory") + + // Downsample methods + .def_static("DownsampleTrajectories", + &traj_eval::TrajectoryEvaluator::DownsampleTrajectories, + py::arg("x_fine"), py::arg("u_fine"), + py::arg("upsample_rate"), + "Downsample state and input trajectories") + .def_static("DownsampleTrajectory", + &traj_eval::TrajectoryEvaluator::DownsampleTrajectory, + py::arg("fine_traj"), py::arg("upsample_rate"), + "Downsample a single trajectory") + + // LCS compatibility checking methods + .def_static( + "CheckCoarseAndFineLCSCompatibility", + &traj_eval::TrajectoryEvaluator::CheckCoarseAndFineLCSCompatibility, + py::arg("coarse_lcs"), py::arg("fine_lcs"), + "Check LCS time discretization compatibility") + .def_static( + "CheckLCSAndTrajectoryCompatibility", + static_cast&, + const std::vector&, + const std::vector&)>( + &traj_eval::TrajectoryEvaluator:: + CheckLCSAndTrajectoryCompatibility), + py::arg("lcs"), py::arg("x"), py::arg("u"), py::arg("lambda"), + "Check trajectory compatibility with LCS (with lambdas)") + .def_static( + "CheckLCSAndTrajectoryCompatibility", + static_cast&, + const std::vector&)>( + &traj_eval::TrajectoryEvaluator:: + CheckLCSAndTrajectoryCompatibility), + py::arg("lcs"), py::arg("x"), py::arg("u"), + "Check trajectory compatibility with LCS (without lambdas)") + .def_static("CheckLCSAndTrajectoryCompatibility", + static_cast&)>( + &traj_eval::TrajectoryEvaluator:: + CheckLCSAndTrajectoryCompatibility), + py::arg("lcs"), py::arg("x"), + "Check state trajectory compatibility with LCS"); +} +} // namespace pyc3 +} // namespace c3 From abf88ab51e1ab6db49f1541d1eb5f79c535b9f14 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Fri, 27 Mar 2026 16:24:53 -0400 Subject: [PATCH 11/17] Tighten variadic templating for more precise input argument checking --- core/traj_eval.h | 175 ++++++++++++++++++++++------------------------- 1 file changed, 83 insertions(+), 92 deletions(-) diff --git a/core/traj_eval.h b/core/traj_eval.h index 311c4826..0bfd1e34 100644 --- a/core/traj_eval.h +++ b/core/traj_eval.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -14,6 +15,18 @@ namespace c3 { namespace traj_eval { +// SFINAE helper to constrain T_des and T_cost to valid types +namespace detail { +template +struct IsDataType + : std::disjunction, + std::is_same>> {}; +template +struct IsCostType + : std::disjunction, + std::is_same>> {}; +} // namespace detail + /// Utility class for trajectory evaluation computations and simulations. class TrajectoryEvaluator { public: @@ -43,92 +56,67 @@ class TrajectoryEvaluator { * explicit instantiations of every argument pattern are needed, which is more * verbose and less flexible than the variadic template approach. */ - template + template ::value && + detail::IsCostType::value>> static double ComputeQuadraticTrajectoryCost( - const std::vector& data, - const std::vector& data_des, - const std::vector& cost_matrices, Rest&&... rest) { - DRAKE_THROW_UNLESS(data.size() == data_des.size()); - DRAKE_THROW_UNLESS(data.size() == cost_matrices.size()); - + const std::vector& data, const T_des& data_des, + const T_cost& cost_matrices, Rest&&... rest) { + double cost = 0.0; + int N = data.size(); int n_data = data[0].size(); - double cost = 0.0; - for (size_t i = 0; i < data.size(); i++) { + // Handle if data_des is provided as a single vector instead of a trajectory + // of vectors. + std::vector data_des_vec; + if constexpr (std::is_same_v>) { + DRAKE_THROW_UNLESS(N == data_des.size()); + data_des_vec = data_des; + } else { + bool is_single_vector = std::is_same_v; + DRAKE_THROW_UNLESS(is_single_vector); + data_des_vec = std::vector(N, data_des); + } + + // Handle if cost_matrices is provided as a single matrix instead of a + // trajectory of matrices. + std::vector cost_matrices_vec; + if constexpr (std::is_same_v>) { + DRAKE_THROW_UNLESS(N == cost_matrices.size()); + cost_matrices_vec = cost_matrices; + } else { + bool is_single_matrix = std::is_same_v; + DRAKE_THROW_UNLESS(is_single_matrix); + cost_matrices_vec = std::vector(N, cost_matrices); + } + + // Sum the costs and check sizes. + for (int i = 0; i < N; i++) { DRAKE_THROW_UNLESS(data[i].size() == n_data); - DRAKE_THROW_UNLESS(data_des[i].size() == n_data); - DRAKE_THROW_UNLESS(cost_matrices[i].rows() == n_data); - DRAKE_THROW_UNLESS(cost_matrices[i].cols() == n_data); + DRAKE_THROW_UNLESS(data_des_vec[i].size() == n_data); + DRAKE_THROW_UNLESS(cost_matrices_vec[i].rows() == n_data); + DRAKE_THROW_UNLESS(cost_matrices_vec[i].cols() == n_data); - cost += (data[i] - data_des[i]).transpose() * cost_matrices[i] * - (data[i] - data_des[i]); + cost += (data[i] - data_des_vec[i]).transpose() * cost_matrices_vec[i] * + (data[i] - data_des_vec[i]); } return cost + ComputeQuadraticTrajectoryCost(std::forward(rest)...); } - /** @brief Special case: the same cost matrix is to be used throughout the - * trajectory so only one is provided. - */ - template - static double ComputeQuadraticTrajectoryCost( - const std::vector& data, - const std::vector& data_des, - const Eigen::MatrixXd& cost_matrix, Rest&&... rest) { - return ComputeQuadraticTrajectoryCost( - data, data_des, std::vector(data.size(), cost_matrix), - std::forward(rest)...); - } - /** @brief Special case: the same desired data is to be used throughout the - * trajectory so only one is provided. - */ - template - static double ComputeQuadraticTrajectoryCost( - const std::vector& data, const Eigen::VectorXd& data_des, - const std::vector& cost_matrices, Rest&&... rest) { - return ComputeQuadraticTrajectoryCost( - data, std::vector(data.size(), data_des), - cost_matrices, std::forward(rest)...); - } - /** @brief Special case: the same desired data and the same cost matrix is to - * be used throughout the trajectory so only one each is provided. - */ - template - static double ComputeQuadraticTrajectoryCost( - const std::vector& data, const Eigen::VectorXd& data_des, - const Eigen::MatrixXd& cost_matrix, Rest&&... rest) { - return ComputeQuadraticTrajectoryCost( - data, std::vector(data.size(), data_des), - std::vector(data.size(), cost_matrix), - std::forward(rest)...); - } /** @brief Special case: no desired data is provided, so it is assumed the * desired data is zero. */ - template + template ::value>> static double ComputeQuadraticTrajectoryCost( - const std::vector& data, - const std::vector& cost_matrices, Rest&&... rest) { + const std::vector& data, const T_cost& cost_matrices, + Rest&&... rest) { return ComputeQuadraticTrajectoryCost( data, std::vector(data.size(), Eigen::VectorXd::Zero(data[0].size())), cost_matrices, std::forward(rest)...); } - /** @brief Special case: no desired data is provided and the same cost matrix - * is to be used throughout the trajectory, so it is assumed the desired data - * is zero and only one cost matrix is provided. - */ - template - static double ComputeQuadraticTrajectoryCost( - const std::vector& data, - const Eigen::MatrixXd& cost_matrix, Rest&&... rest) { - return ComputeQuadraticTrajectoryCost( - data, - std::vector(data.size(), - Eigen::VectorXd::Zero(data[0].size())), - std::vector(data.size(), cost_matrix), - std::forward(rest)...); - } /** @brief Special case: a C3 object is the input argument, and the cost is to * be computed based on the trajectories contained in the C3 object and the * cost matrices contained in the C3 object for state and input costs. @@ -153,9 +141,11 @@ class TrajectoryEvaluator { * trajectories. If the C3 object is configured to compute input costs based * on (u-u_prev), this function throws an error. */ - static double ComputeQuadraticTrajectoryCost( - C3* c3, const std::vector& Q, - const std::vector& R) { + template ::value && + detail::IsCostType::value>> + static double ComputeQuadraticTrajectoryCost(C3* c3, const T_Q_cost& Q, + const T_R_cost& R) { DRAKE_THROW_UNLESS(c3->GetPenalizeInputChange() == false); // Get the trajectories (solved for state and input, and desired for state). @@ -176,29 +166,30 @@ class TrajectoryEvaluator { DRAKE_THROW_UNLESS(x_des.size() == x.size()); DRAKE_THROW_UNLESS(x.size() == u.size() + 1); - DRAKE_THROW_UNLESS(Q.size() == x.size()); - DRAKE_THROW_UNLESS(R.size() == u.size()); - return ComputeQuadraticTrajectoryCost(x, x_des, Q, u, R); - } - /** @brief Special cases (3x): a C3 object and singular cost matrices (either - * Q, or R, or both) are the input arguments. */ - static double ComputeQuadraticTrajectoryCost( - C3* c3, const Eigen::MatrixXd& Q, const std::vector& R) { - return ComputeQuadraticTrajectoryCost( - c3, std::vector(c3->GetStateSolution().size() + 1, Q), - R); - } - static double ComputeQuadraticTrajectoryCost( - C3* c3, const std::vector& Q, const Eigen::MatrixXd& R) { - return ComputeQuadraticTrajectoryCost( - c3, Q, std::vector(c3->GetInputSolution().size(), R)); - } - static double ComputeQuadraticTrajectoryCost(C3* c3, const Eigen::MatrixXd& Q, - const Eigen::MatrixXd& R) { - return ComputeQuadraticTrajectoryCost( - c3, std::vector(c3->GetStateSolution().size() + 1, Q), - std::vector(c3->GetInputSolution().size(), R)); + // Handle if Q is provided as a single matrix instead of a trajectory of Qs. + std::vector Q_vec; + if constexpr (std::is_same_v>) { + DRAKE_THROW_UNLESS(x.size() == Q.size()); + Q_vec = Q; + } else { + bool is_single_matrix = std::is_same_v; + DRAKE_THROW_UNLESS(is_single_matrix); + Q_vec = std::vector(x.size(), Q); + } + + // Handle if R is provided as a single matrix instead of a trajectory of Rs. + std::vector R_vec; + if constexpr (std::is_same_v>) { + DRAKE_THROW_UNLESS(u.size() == R.size()); + R_vec = R; + } else { + bool is_single_matrix = std::is_same_v; + DRAKE_THROW_UNLESS(is_single_matrix); + R_vec = std::vector(u.size(), R); + } + + return ComputeQuadraticTrajectoryCost(x, x_des, Q_vec, u, R_vec); } /** From 19e7db2321b919787d30906928e0f36a45d4e9f1 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Tue, 31 Mar 2026 12:14:11 -0400 Subject: [PATCH 12/17] Consolidate function overloads with optional input arguments; expose more input argument options for bindings --- bindings/pyc3/traj_eval_py.cc | 50 +++++++++++++++++++---------------- core/traj_eval.cc | 49 +++++++++++++++------------------- core/traj_eval.h | 34 ++++++++++-------------- 3 files changed, 63 insertions(+), 70 deletions(-) diff --git a/bindings/pyc3/traj_eval_py.cc b/bindings/pyc3/traj_eval_py.cc index b2bd7ecb..4ca74726 100644 --- a/bindings/pyc3/traj_eval_py.cc +++ b/bindings/pyc3/traj_eval_py.cc @@ -51,6 +51,23 @@ PYBIND11_MODULE(traj_eval, m) { &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), py::arg("data"), py::arg("data_des"), py::arg("cost_matrix"), "Compute quadratic cost with single cost matrix") + .def_static( + "ComputeQuadraticTrajectoryCost", + static_cast&, + const Eigen::VectorXd&, + const std::vector&)>( + &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + py::arg("data"), py::arg("data_des"), py::arg("cost_matrices"), + "Compute quadratic cost with single desired vector") + .def_static( + "ComputeQuadraticTrajectoryCost", + static_cast&, + const Eigen::VectorXd&, + const Eigen::MatrixXd&)>( + &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost), + py::arg("data"), py::arg("data_des"), py::arg("cost_matrix"), + "Compute quadratic cost with single desired vector and single cost " + "matrix") .def_static( "ComputeQuadraticTrajectoryCost", static_cast&, @@ -177,11 +194,11 @@ PYBIND11_MODULE(traj_eval, m) { .def_static("DownsampleTrajectories", &traj_eval::TrajectoryEvaluator::DownsampleTrajectories, py::arg("x_fine"), py::arg("u_fine"), - py::arg("upsample_rate"), + py::arg("downsample_rate"), "Downsample state and input trajectories") .def_static("DownsampleTrajectory", &traj_eval::TrajectoryEvaluator::DownsampleTrajectory, - py::arg("fine_traj"), py::arg("upsample_rate"), + py::arg("fine_traj"), py::arg("downsample_rate"), "Downsample a single trajectory") // LCS compatibility checking methods @@ -190,30 +207,17 @@ PYBIND11_MODULE(traj_eval, m) { &traj_eval::TrajectoryEvaluator::CheckCoarseAndFineLCSCompatibility, py::arg("coarse_lcs"), py::arg("fine_lcs"), "Check LCS time discretization compatibility") - .def_static( - "CheckLCSAndTrajectoryCompatibility", - static_cast&, - const std::vector&, - const std::vector&)>( - &traj_eval::TrajectoryEvaluator:: - CheckLCSAndTrajectoryCompatibility), - py::arg("lcs"), py::arg("x"), py::arg("u"), py::arg("lambda"), - "Check trajectory compatibility with LCS (with lambdas)") - .def_static( - "CheckLCSAndTrajectoryCompatibility", - static_cast&, - const std::vector&)>( - &traj_eval::TrajectoryEvaluator:: - CheckLCSAndTrajectoryCompatibility), - py::arg("lcs"), py::arg("x"), py::arg("u"), - "Check trajectory compatibility with LCS (without lambdas)") .def_static("CheckLCSAndTrajectoryCompatibility", - static_cast&)>( + static_cast&, + const std::optional>&, + const std::optional>&)>( &traj_eval::TrajectoryEvaluator:: CheckLCSAndTrajectoryCompatibility), - py::arg("lcs"), py::arg("x"), - "Check state trajectory compatibility with LCS"); + py::arg("lcs"), py::arg("x"), py::arg("u") = py::none(), + py::arg("lambda") = py::none(), + "Check trajectory compatibility with LCS; inputs and lambdas " + "are optional"); } } // namespace pyc3 } // namespace c3 diff --git a/core/traj_eval.cc b/core/traj_eval.cc index b07a1374..c3847d5a 100644 --- a/core/traj_eval.cc +++ b/core/traj_eval.cc @@ -159,14 +159,14 @@ TrajectoryEvaluator::ZeroOrderHoldTrajectories(const vector& x_coarse, } vector TrajectoryEvaluator::DownsampleTrajectory( - const vector& fine_traj, const int& upsample_rate) { - DRAKE_THROW_UNLESS(fine_traj.size() % upsample_rate == 0); - int N = fine_traj.size() / upsample_rate; + const vector& fine_traj, const int& downsample_rate) { + DRAKE_THROW_UNLESS(fine_traj.size() % downsample_rate == 0); + int N = fine_traj.size() / downsample_rate; // Downsample the fine trajectory. vector coarse_traj(N, VectorXd::Zero(fine_traj[0].size())); for (int i = 0; i < N; i++) { - coarse_traj[i] = fine_traj[i * upsample_rate]; + coarse_traj[i] = fine_traj[i * downsample_rate]; } return coarse_traj; } @@ -174,12 +174,12 @@ vector TrajectoryEvaluator::DownsampleTrajectory( std::pair, vector> TrajectoryEvaluator::DownsampleTrajectories(const vector& x_fine, const vector& u_fine, - const int& upsample_rate) { + const int& downsample_rate) { DRAKE_THROW_UNLESS(x_fine.size() == u_fine.size() + 1); vector x_coarse = DownsampleTrajectory( - vector(x_fine.begin(), x_fine.end() - 1), upsample_rate); + vector(x_fine.begin(), x_fine.end() - 1), downsample_rate); x_coarse.push_back(x_fine.back()); - vector u_coarse = DownsampleTrajectory(u_fine, upsample_rate); + vector u_coarse = DownsampleTrajectory(u_fine, downsample_rate); return std::make_pair(x_coarse, u_coarse); } @@ -198,32 +198,27 @@ int TrajectoryEvaluator::CheckCoarseAndFineLCSCompatibility( void TrajectoryEvaluator::CheckLCSAndTrajectoryCompatibility( const LCS& lcs, const std::vector& x, - const std::vector& u, - const std::vector& lambda) { - DRAKE_THROW_UNLESS(lcs.N() == x.size() - 1 && lcs.N() == u.size() && - lcs.N() == lambda.size()); + const std::optional>& u, + const std::optional>& lambda) { + DRAKE_THROW_UNLESS(lcs.N() == x.size() - 1); + if (u.has_value()) { + DRAKE_THROW_UNLESS(lcs.N() == u->size()); + } + if (lambda.has_value()) { + DRAKE_THROW_UNLESS(lcs.N() == lambda->size()); + } for (int i = 0; i < lcs.N() + 1; i++) { DRAKE_THROW_UNLESS(x[i].size() == lcs.num_states()); if (i < lcs.N()) { - DRAKE_THROW_UNLESS(u[i].size() == lcs.num_inputs()); - DRAKE_THROW_UNLESS(lambda[i].size() == lcs.num_lambdas()); + if (u.has_value()) { + DRAKE_THROW_UNLESS(u->at(i).size() == lcs.num_inputs()); + } + if (lambda.has_value()) { + DRAKE_THROW_UNLESS(lambda->at(i).size() == lcs.num_lambdas()); + } } } } -void TrajectoryEvaluator::CheckLCSAndTrajectoryCompatibility( - const LCS& lcs, const std::vector& x, - const std::vector& u) { - return CheckLCSAndTrajectoryCompatibility( - lcs, x, u, vector(lcs.N(), VectorXd::Zero(lcs.num_lambdas()))); -} - -void TrajectoryEvaluator::CheckLCSAndTrajectoryCompatibility( - const LCS& lcs, const std::vector& x) { - return CheckLCSAndTrajectoryCompatibility( - lcs, x, vector(lcs.N(), VectorXd::Zero(lcs.num_inputs())), - vector(lcs.N(), VectorXd::Zero(lcs.num_lambdas()))); -} - } // namespace traj_eval } // namespace c3 diff --git a/core/traj_eval.h b/core/traj_eval.h index 0bfd1e34..3a63441b 100644 --- a/core/traj_eval.h +++ b/core/traj_eval.h @@ -297,19 +297,20 @@ class TrajectoryEvaluator { * The same compatibility requirements apply as for * ZeroOrderHoldTrajectoryToFinerLCS. * - * @param x_fine Fine state trajectory (length (N*upsample_rate)+1) - * @param u_fine Fine input trajectory (length N*upsample_rate) - * @param upsample_rate Integer multiple for time discretization + * @param x_fine Fine state trajectory (length (N*downsample_rate)+1) + * @param u_fine Fine input trajectory (length N*downsample_rate) + * @param downsample_rate Integer multiple for time discretization * @return Pair of 1) coarse state trajectory (length N+1) and 2) coarse input * trajectory (length N) */ static std::pair, std::vector> DownsampleTrajectories(const std::vector& x_fine, const std::vector& u_fine, - const int& upsample_rate); + const int& downsample_rate); /** @brief Same as above but only one trajectory is provided and processed. */ static std::vector DownsampleTrajectory( - const std::vector& fine_traj, const int& upsample_rate); + const std::vector& fine_traj, + const int& downsample_rate); /** * @brief Check that two LCSs are compatible with each other, i.e. that the @@ -330,27 +331,20 @@ class TrajectoryEvaluator { const LCS& fine_lcs); /** - * @brief Check that a trajectory of states, inputs, and contact forces is - * compatible with an LCS, i.e. that the trajectory has the correct number of - * time steps and that the dimensions of the states, inputs, and contact - * forces match the dimensions of the LCS. + * @brief Check that a trajectory of states (and optionally inputs and contact + * forces) is compatible with an LCS, i.e. that the trajectory has the correct + * number of time steps and that the dimensions of the states, inputs, and + * contact forces match the dimensions of the LCS. * * @param lcs LCS system * @param x State trajectory (length N+1) - * @param u Input trajectory (length N) - * @param lambda Contact force trajectory (length N) + * @param u (Optional) Input trajectory (length N) + * @param lambda (Optional) Contact force trajectory (length N) */ static void CheckLCSAndTrajectoryCompatibility( const LCS& lcs, const std::vector& x, - const std::vector& u, - const std::vector& lambda); - /** @brief Special case: no lambdas provided. */ - static void CheckLCSAndTrajectoryCompatibility( - const LCS& lcs, const std::vector& x, - const std::vector& u); - /** @brief Special case: no inputs or lambdas provided. */ - static void CheckLCSAndTrajectoryCompatibility( - const LCS& lcs, const std::vector& x); + const std::optional>& u = std::nullopt, + const std::optional>& lambda = std::nullopt); private: /** @brief Special case of ComputeQuadraticTrajectoryCost: no trajectory to From 1aa492822baebf3c02ab0057380fc10bed87a789 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Wed, 8 Apr 2026 14:14:56 -0400 Subject: [PATCH 13/17] Take in LCS simulation configuration for LCS-based PD control --- bindings/pyc3/traj_eval_py.cc | 10 +++++++--- core/traj_eval.cc | 19 +++++++++---------- core/traj_eval.h | 19 +++++++++++-------- 3 files changed, 27 insertions(+), 21 deletions(-) diff --git a/bindings/pyc3/traj_eval_py.cc b/bindings/pyc3/traj_eval_py.cc index 4ca74726..fb42fb2f 100644 --- a/bindings/pyc3/traj_eval_py.cc +++ b/bindings/pyc3/traj_eval_py.cc @@ -123,20 +123,24 @@ PYBIND11_MODULE(traj_eval, m) { std::vector> (*)( const std::vector&, const std::vector&, const Eigen::VectorXd&, - const Eigen::VectorXd&, const LCS&)>( + const Eigen::VectorXd&, const LCS&, + const c3::LCSSimulateConfig&)>( &traj_eval::TrajectoryEvaluator::SimulatePDControlWithLCS), py::arg("x_plan"), py::arg("u_plan"), py::arg("Kp"), py::arg("Kd"), - py::arg("lcs"), "Simulate PD control with an LCS") + py::arg("lcs"), py::arg("config") = c3::LCSSimulateConfig(), + "Simulate PD control with an LCS") .def_static( "SimulatePDControlWithLCS", static_cast, std::vector> (*)( const std::vector&, const std::vector&, const Eigen::VectorXd&, - const Eigen::VectorXd&, const LCS&, const LCS&)>( + const Eigen::VectorXd&, const LCS&, const LCS&, + const c3::LCSSimulateConfig&)>( &traj_eval::TrajectoryEvaluator::SimulatePDControlWithLCS), py::arg("x_plan"), py::arg("u_plan"), py::arg("Kp"), py::arg("Kd"), py::arg("coarse_lcs"), py::arg("fine_lcs"), + py::arg("config") = c3::LCSSimulateConfig(), "Simulate PD control with coarse and fine LCSs") // SimulateLCSOverTrajectory overloads diff --git a/core/traj_eval.cc b/core/traj_eval.cc index c3847d5a..a54dce8c 100644 --- a/core/traj_eval.cc +++ b/core/traj_eval.cc @@ -20,7 +20,8 @@ TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, const vector& u_plan, const VectorXd& Kp, const VectorXd& Kd, - const LCS& lcs) { + const LCS& lcs, + const LCSSimulateConfig& config) { CheckLCSAndTrajectoryCompatibility(lcs, x_plan, u_plan); int N = lcs.N(); @@ -56,18 +57,16 @@ TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, for (int i = 0; i < N; i++) { u[i] = u_plan[i] + Kp_mat * (x_plan[i] - x[i]) + Kd_mat * (x_plan[i] - x[i]); - x[i + 1] = lcs.Simulate(x[i], u[i]); + x[i + 1] = lcs.Simulate(x[i], u[i], config); } return std::make_pair(x, u); } std::pair, vector> -TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, - const vector& u_plan, - const VectorXd& Kp, - const VectorXd& Kd, - const LCS& coarse_lcs, - const LCS& fine_lcs) { +TrajectoryEvaluator::SimulatePDControlWithLCS( + const vector& x_plan, const vector& u_plan, + const VectorXd& Kp, const VectorXd& Kd, const LCS& coarse_lcs, + const LCS& fine_lcs, const LCSSimulateConfig& config) { int upsample_rate = CheckCoarseAndFineLCSCompatibility(coarse_lcs, fine_lcs); // Zero-order hold the planned trajectory to match the finer time @@ -76,8 +75,8 @@ TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, ZeroOrderHoldTrajectories(x_plan, u_plan, upsample_rate); // Do PD control with the finer trajectory and LCS. - auto [x_sim_fine, u_sim_fine] = - SimulatePDControlWithLCS(x_plan_finer, u_plan_finer, Kp, Kd, fine_lcs); + auto [x_sim_fine, u_sim_fine] = SimulatePDControlWithLCS( + x_plan_finer, u_plan_finer, Kp, Kd, fine_lcs, config); // Downsample the resulting trajectory back to the original time // discretization. diff --git a/core/traj_eval.h b/core/traj_eval.h index 3a63441b..6e80043e 100644 --- a/core/traj_eval.h +++ b/core/traj_eval.h @@ -205,13 +205,15 @@ class TrajectoryEvaluator { * @param Kd Derivative gains (length n_x, with exactly k non-zero * entries) * @param lcs LCS system to simulate + * @param config Configuration for simulating the LCS * @return Pair of (simulated states, simulated inputs) */ static std::pair, std::vector> - SimulatePDControlWithLCS(const std::vector& x_plan, - const std::vector& u_plan, - const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd, - const LCS& lcs); + SimulatePDControlWithLCS( + const std::vector& x_plan, + const std::vector& u_plan, const Eigen::VectorXd& Kp, + const Eigen::VectorXd& Kd, const LCS& lcs, + const LCSSimulateConfig& config = LCSSimulateConfig()); /** * @brief Special case: simulate plans from a coarser LCS with a finer LCS. * The returned trajectory is downsampled back to be compatible with the @@ -219,10 +221,11 @@ class TrajectoryEvaluator { * and input plans must be compatible with the coarser LCS. */ static std::pair, std::vector> - SimulatePDControlWithLCS(const std::vector& x_plan, - const std::vector& u_plan, - const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd, - const LCS& coarse_lcs, const LCS& fine_lcs); + SimulatePDControlWithLCS( + const std::vector& x_plan, + const std::vector& u_plan, const Eigen::VectorXd& Kp, + const Eigen::VectorXd& Kd, const LCS& coarse_lcs, const LCS& fine_lcs, + const LCSSimulateConfig& config = LCSSimulateConfig()); /** * @brief Simulate an LCS forward from an initial condition over a trajectory From 9cacea5c2c8ee10f0efabd1b022bb39737139798 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Wed, 8 Apr 2026 14:42:02 -0400 Subject: [PATCH 14/17] Add input to turn on/off feedforward term in PD simulation --- bindings/pyc3/traj_eval_py.cc | 8 +++++--- core/traj_eval.cc | 21 +++++++++++---------- core/traj_eval.h | 2 ++ 3 files changed, 18 insertions(+), 13 deletions(-) diff --git a/bindings/pyc3/traj_eval_py.cc b/bindings/pyc3/traj_eval_py.cc index fb42fb2f..4773d5c9 100644 --- a/bindings/pyc3/traj_eval_py.cc +++ b/bindings/pyc3/traj_eval_py.cc @@ -123,11 +123,12 @@ PYBIND11_MODULE(traj_eval, m) { std::vector> (*)( const std::vector&, const std::vector&, const Eigen::VectorXd&, - const Eigen::VectorXd&, const LCS&, + const Eigen::VectorXd&, const LCS&, const bool&, const c3::LCSSimulateConfig&)>( &traj_eval::TrajectoryEvaluator::SimulatePDControlWithLCS), py::arg("x_plan"), py::arg("u_plan"), py::arg("Kp"), py::arg("Kd"), - py::arg("lcs"), py::arg("config") = c3::LCSSimulateConfig(), + py::arg("lcs"), py::arg("use_feedforward") = true, + py::arg("config") = c3::LCSSimulateConfig(), "Simulate PD control with an LCS") .def_static( "SimulatePDControlWithLCS", @@ -135,11 +136,12 @@ PYBIND11_MODULE(traj_eval, m) { std::vector> (*)( const std::vector&, const std::vector&, const Eigen::VectorXd&, - const Eigen::VectorXd&, const LCS&, const LCS&, + const Eigen::VectorXd&, const LCS&, const LCS&, const bool&, const c3::LCSSimulateConfig&)>( &traj_eval::TrajectoryEvaluator::SimulatePDControlWithLCS), py::arg("x_plan"), py::arg("u_plan"), py::arg("Kp"), py::arg("Kd"), py::arg("coarse_lcs"), py::arg("fine_lcs"), + py::arg("use_feedforward") = true, py::arg("config") = c3::LCSSimulateConfig(), "Simulate PD control with coarse and fine LCSs") diff --git a/core/traj_eval.cc b/core/traj_eval.cc index a54dce8c..15c4a72b 100644 --- a/core/traj_eval.cc +++ b/core/traj_eval.cc @@ -16,12 +16,10 @@ using Eigen::VectorXd; using std::vector; std::pair, vector> -TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, - const vector& u_plan, - const VectorXd& Kp, - const VectorXd& Kd, - const LCS& lcs, - const LCSSimulateConfig& config) { +TrajectoryEvaluator::SimulatePDControlWithLCS( + const vector& x_plan, const vector& u_plan, + const VectorXd& Kp, const VectorXd& Kd, const LCS& lcs, + const bool& use_feedforward, const LCSSimulateConfig& config) { CheckLCSAndTrajectoryCompatibility(lcs, x_plan, u_plan); int N = lcs.N(); @@ -55,8 +53,10 @@ TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, x[0] = x_plan[0]; for (int i = 0; i < N; i++) { - u[i] = - u_plan[i] + Kp_mat * (x_plan[i] - x[i]) + Kd_mat * (x_plan[i] - x[i]); + u[i] = Kp_mat * (x_plan[i] - x[i]) + Kd_mat * (x_plan[i] - x[i]); + if (use_feedforward) { + u[i] += u_plan[i]; + } x[i + 1] = lcs.Simulate(x[i], u[i], config); } return std::make_pair(x, u); @@ -66,7 +66,8 @@ std::pair, vector> TrajectoryEvaluator::SimulatePDControlWithLCS( const vector& x_plan, const vector& u_plan, const VectorXd& Kp, const VectorXd& Kd, const LCS& coarse_lcs, - const LCS& fine_lcs, const LCSSimulateConfig& config) { + const LCS& fine_lcs, const bool& use_feedforward, + const LCSSimulateConfig& config) { int upsample_rate = CheckCoarseAndFineLCSCompatibility(coarse_lcs, fine_lcs); // Zero-order hold the planned trajectory to match the finer time @@ -76,7 +77,7 @@ TrajectoryEvaluator::SimulatePDControlWithLCS( // Do PD control with the finer trajectory and LCS. auto [x_sim_fine, u_sim_fine] = SimulatePDControlWithLCS( - x_plan_finer, u_plan_finer, Kp, Kd, fine_lcs, config); + x_plan_finer, u_plan_finer, Kp, Kd, fine_lcs, use_feedforward, config); // Downsample the resulting trajectory back to the original time // discretization. diff --git a/core/traj_eval.h b/core/traj_eval.h index 6e80043e..d0ca3e0e 100644 --- a/core/traj_eval.h +++ b/core/traj_eval.h @@ -213,6 +213,7 @@ class TrajectoryEvaluator { const std::vector& x_plan, const std::vector& u_plan, const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd, const LCS& lcs, + const bool& use_feedforward = true, const LCSSimulateConfig& config = LCSSimulateConfig()); /** * @brief Special case: simulate plans from a coarser LCS with a finer LCS. @@ -225,6 +226,7 @@ class TrajectoryEvaluator { const std::vector& x_plan, const std::vector& u_plan, const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd, const LCS& coarse_lcs, const LCS& fine_lcs, + const bool& use_feedforward = true, const LCSSimulateConfig& config = LCSSimulateConfig()); /** From ae1afa2a690bc99241b0b1b6f7988f14f96ddd19 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Wed, 22 Apr 2026 14:54:14 -0400 Subject: [PATCH 15/17] Add new PD simulation option without provided feedforward terms; include relevant bindings and tests --- bindings/pyc3/test/test_traj_eval.py | 105 ++++++++++++++++- bindings/pyc3/traj_eval_py.cc | 30 +++++ core/test/core_test.cc | 163 ++++++++++++++++----------- core/traj_eval.cc | 27 +++++ core/traj_eval.h | 18 +++ 5 files changed, 273 insertions(+), 70 deletions(-) diff --git a/bindings/pyc3/test/test_traj_eval.py b/bindings/pyc3/test/test_traj_eval.py index 5c92352f..f8698598 100644 --- a/bindings/pyc3/test/test_traj_eval.py +++ b/bindings/pyc3/test/test_traj_eval.py @@ -46,7 +46,9 @@ def setUp(self): self.Q_matrices = [ np.eye(self.n_x, dtype=np.float64) for _ in range(self.N + 1) ] - self.R_matrices = [np.eye(self.n_u, dtype=np.float64) for _ in range(self.N)] + self.R_matrices = [ + np.eye(self.n_u, dtype=np.float64) for _ in range(self.N) + ] def test_compute_quadratic_trajectory_cost_basic(self): # Test with single matrices @@ -105,14 +107,18 @@ def test_compute_quadratic_trajectory_cost_with_c3(self): opts.M = 100.0 opts.gamma = 1.0 opts.rho_scale = 1.0 - opts.penalize_input_change = False # This should prevent the constraint failure + opts.penalize_input_change = ( + False # This should prevent the constraint failure + ) costs = c3.C3.CreateCostMatricesFromC3Options(opts, self.N) solver = c3.C3QP(self.lcs, costs, self.x_des, opts) solver.Solve(np.zeros(self.n_x)) # Test cost computation from C3 solver - cost = traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost(solver) + cost = traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + solver + ) self.assertGreaterEqual(cost, 0) # Test with custom matrices @@ -143,11 +149,49 @@ def test_simulate_pd_control_with_lcs(self): for u in u_sim: self.assertEqual(len(u), self.n_u) + def test_simulate_pd_control_with_lcs_no_feedforward_overload(self): + # Test the overload without u_plan. + Kp = np.zeros(self.n_x) + Kd = np.zeros(self.n_x) + Kp[0] = 10.0 + Kp[1] = 10.0 + Kd[2] = 1.0 + Kd[3] = 1.0 + + x_sim, u_sim = traj_eval.TrajectoryEvaluator.SimulatePDControlWithLCS( + self.x_traj, Kp, Kd, self.lcs + ) + + self.assertEqual(len(x_sim), self.N + 1) + self.assertEqual(len(u_sim), self.N) + for x in x_sim: + self.assertEqual(len(x), self.n_x) + for u in u_sim: + self.assertEqual(len(u), self.n_u) + + # This overload should match explicit zero feedforward behavior. + zero_u_plan = [np.zeros(self.n_u) for _ in range(self.N)] + x_sim_zero, u_sim_zero = ( + traj_eval.TrajectoryEvaluator.SimulatePDControlWithLCS( + self.x_traj, + zero_u_plan, + Kp, + Kd, + self.lcs, + False, + ) + ) + for x_a, x_b in zip(x_sim, x_sim_zero): + np.testing.assert_allclose(x_a, x_b) + for u_a, u_b in zip(u_sim, u_sim_zero): + np.testing.assert_allclose(u_a, u_b) + def test_simulate_pd_control_with_coarse_fine_lcs(self): # Create fine LCS with smaller dt, following core_test.cc pattern fine_lcs = c3.LCS( np.eye(self.n_x), - np.ones((self.n_x, self.n_u)) / 2.0, # Scale B matrix for finer time step + np.ones((self.n_x, self.n_u)) + / 2.0, # Scale B matrix for finer time step np.ones((self.n_x, self.n_lambda)), np.zeros(self.n_x), np.ones((self.n_lambda, self.n_x)), @@ -173,6 +217,55 @@ def test_simulate_pd_control_with_coarse_fine_lcs(self): self.assertEqual(len(x_sim), self.N + 1) self.assertEqual(len(u_sim), self.N) + def test_simulate_pd_control_with_coarse_fine_lcs_no_feedforward_overload( + self, + ): + # Test the coarse/fine overload without u_plan. + fine_lcs = c3.LCS( + np.eye(self.n_x), + np.ones((self.n_x, self.n_u)) / 2.0, + np.ones((self.n_x, self.n_lambda)), + np.zeros(self.n_x), + np.ones((self.n_lambda, self.n_x)), + np.eye(self.n_lambda), + np.ones((self.n_lambda, self.n_u)), + np.ones(self.n_lambda), + self.N * 2, + self.dt / 2, + ) + + Kp = np.zeros(self.n_x) + Kd = np.zeros(self.n_x) + Kp[0] = 10.0 + Kp[1] = 10.0 + Kd[2] = 1.0 + Kd[3] = 1.0 + + x_sim, u_sim = traj_eval.TrajectoryEvaluator.SimulatePDControlWithLCS( + self.x_traj, Kp, Kd, self.lcs, fine_lcs + ) + + self.assertEqual(len(x_sim), self.N + 1) + self.assertEqual(len(u_sim), self.N) + + # This overload should match explicit zero feedforward behavior. + zero_u_plan = [np.zeros(self.n_u) for _ in range(self.N)] + x_sim_zero, u_sim_zero = ( + traj_eval.TrajectoryEvaluator.SimulatePDControlWithLCS( + self.x_traj, + zero_u_plan, + Kp, + Kd, + self.lcs, + fine_lcs, + False, + ) + ) + for x_a, x_b in zip(x_sim, x_sim_zero): + np.testing.assert_allclose(x_a, x_b) + for u_a, u_b in zip(u_sim, u_sim_zero): + np.testing.assert_allclose(u_a, u_b) + def test_simulate_lcs_over_trajectory(self): config = c3.LCSSimulateConfig() @@ -322,7 +415,9 @@ def test_trajectory_compatibility_errors(self): ) # Test with wrong length - short_x = [np.ones(self.n_x) for _ in range(self.N)] # Missing one state + short_x = [ + np.ones(self.n_x) for _ in range(self.N) + ] # Missing one state with self.assertRaises(Exception): traj_eval.TrajectoryEvaluator.CheckLCSAndTrajectoryCompatibility( diff --git a/bindings/pyc3/traj_eval_py.cc b/bindings/pyc3/traj_eval_py.cc index a2b718d9..223956d1 100644 --- a/bindings/pyc3/traj_eval_py.cc +++ b/bindings/pyc3/traj_eval_py.cc @@ -155,6 +155,8 @@ PYBIND11_MODULE(traj_eval, m) { "Compute cost from C3 with custom cost matrices.") // SimulatePDControlWithLCS overloads + // Binding: (vector, vector, VectorXd, VectorXd, + // LCS, bool, LCSSimulateConfig) .def_static( "SimulatePDControlWithLCS", static_cast, @@ -168,6 +170,20 @@ PYBIND11_MODULE(traj_eval, m) { py::arg("lcs"), py::arg("use_feedforward") = true, py::arg("config") = c3::LCSSimulateConfig(), "Simulate PD control with an LCS") + // Binding: (vector, VectorXd, VectorXd, LCS, + // LCSSimulateConfig) + .def_static( + "SimulatePDControlWithLCS", + static_cast, + std::vector> (*) ( + const std::vector&, const Eigen::VectorXd&, + const Eigen::VectorXd&, const LCS&, const c3::LCSSimulateConfig&)>( + &traj_eval::TrajectoryEvaluator::SimulatePDControlWithLCS), + py::arg("x_plan"), py::arg("Kp"), py::arg("Kd"), + py::arg("lcs"), py::arg("config") = c3::LCSSimulateConfig(), + "Simulate PD control with an LCS without feedforward") + // Binding: (vector, vector, VectorXd, VectorXd, + // LCS, LCS, bool, LCSSimulateConfig) .def_static( "SimulatePDControlWithLCS", static_cast, @@ -182,6 +198,20 @@ PYBIND11_MODULE(traj_eval, m) { py::arg("use_feedforward") = true, py::arg("config") = c3::LCSSimulateConfig(), "Simulate PD control with coarse and fine LCSs") + // Binding: (vector, VectorXd, VectorXd, LCS, LCS, + // LCSSimulateConfig) + .def_static( + "SimulatePDControlWithLCS", + static_cast, + std::vector> (*) ( + const std::vector&, const Eigen::VectorXd&, + const Eigen::VectorXd&, const LCS&, const LCS&, + const c3::LCSSimulateConfig&)>( + &traj_eval::TrajectoryEvaluator::SimulatePDControlWithLCS), + py::arg("x_plan"), py::arg("Kp"), py::arg("Kd"), + py::arg("coarse_lcs"), py::arg("fine_lcs"), + py::arg("config") = c3::LCSSimulateConfig(), + "Simulate PD control with coarse and fine LCSs without feedforward") // SimulateLCSOverTrajectory overloads .def_static( diff --git a/core/test/core_test.cc b/core/test/core_test.cc index 68bff97f..9f6babea 100644 --- a/core/test/core_test.cc +++ b/core/test/core_test.cc @@ -78,7 +78,7 @@ TEST_F(C3CartpoleTest, InitializationTest) { // Test if GetLinearConstraints is working as expected TEST_F(C3CartpoleTest, LinearConstraintsTest) { - std::vector user_constraints; + vector user_constraints; ASSERT_NO_THROW({ user_constraints = pOpt->GetLinearConstraints(); }); ASSERT_EQ(user_constraints.size(), 0); } @@ -116,7 +116,7 @@ TEST_P(C3CartpoleTestParameterizedLinearConstraints, LinearConstraintsTest) { pOpt->AddLinearConstraint(Al, lb, ub, constraint_type); } - std::vector user_constraints = + vector user_constraints = pOpt->GetLinearConstraints(); // Number of constraints must be N-1 for state and N for input and lambda EXPECT_EQ(user_constraints.size(), num_of_new_constraints); @@ -151,7 +151,7 @@ TEST_F(C3CartpoleTest, RemoveLinearConstraintsTest) { pOpt->AddLinearConstraint(Al, lb, ub, c3::ConstraintVariable::STATE); - std::vector user_constraints = + vector user_constraints = pOpt->GetLinearConstraints(); EXPECT_EQ(user_constraints.size(), N - 1); @@ -176,8 +176,7 @@ TEST_F(C3CartpoleTest, UpdateTargetTest) { pOpt->UpdateTarget(xdesired); - std::vector target_costs = - pOpt->GetTargetCost(); + vector target_costs = pOpt->GetTargetCost(); EXPECT_EQ(target_costs.size(), N + 1); for (int i = 0; i < N + 1; ++i) { @@ -213,8 +212,7 @@ TEST_F(C3CartpoleTest, UpdateCostMatrix) { } // Ensure target state costs are updated - std::vector target_costs = - pOpt->GetTargetCost(); + vector target_costs = pOpt->GetTargetCost(); for (int i = 0; i < N + 1; ++i) { // Quadratic Q and b cost matrices should be updated @@ -250,7 +248,7 @@ TEST_P(C3CartpoleTestParameterizedScalingLCSTest, ScalingLCSTest) { LinEq.block(0, n, n, m) = pSystem->D().at(0); LinEq.block(0, n + m, n, k) = pSystem->B().at(0); - std::vector dynamic_constraints = + vector dynamic_constraints = optimizer.GetDynamicConstraints(); for (int i = 0; i < N; ++i) { // Linear Equality A matrix should be updated @@ -282,9 +280,9 @@ TEST_F(C3CartpoleTest, ZSolStaleTest) { int timesteps = 5; // number of timesteps for the simulation // Create state and input arrays - std::vector z(timesteps, VectorXd::Zero(n + m + k)); - std::vector state(timesteps, VectorXd::Zero(n)); - std::vector input(timesteps, VectorXd::Zero(k)); + vector z(timesteps, VectorXd::Zero(n + m + k)); + vector state(timesteps, VectorXd::Zero(n)); + vector input(timesteps, VectorXd::Zero(k)); state[0] << 0.1, -0.5, 0.5, -0.4; // initial state with contact to right wall @@ -326,12 +324,12 @@ TEST_F(C3CartpoleTest, WarmStartSmokeTest) { class TrajectoryEvaluatorTest : public testing::Test {}; TEST_F(TrajectoryEvaluatorTest, QuadraticCostMatchesManual) { - std::vector x(2, VectorXd::Zero(1)); - std::vector u(1, VectorXd::Zero(1)); - std::vector lambda(1, VectorXd::Zero(1)); - std::vector x_des(2, VectorXd::Zero(1)); - std::vector u_des(1, VectorXd::Zero(1)); - std::vector lambda_des(1, VectorXd::Zero(1)); + vector x(2, VectorXd::Zero(1)); + vector u(1, VectorXd::Zero(1)); + vector lambda(1, VectorXd::Zero(1)); + vector x_des(2, VectorXd::Zero(1)); + vector u_des(1, VectorXd::Zero(1)); + vector lambda_des(1, VectorXd::Zero(1)); x[0](0) = 1.0; x[1](0) = 2.0; @@ -342,9 +340,9 @@ TEST_F(TrajectoryEvaluatorTest, QuadraticCostMatchesManual) { u_des[0](0) = 2.0; lambda_des[0](0) = 0.0; - std::vector Q(2, MatrixXd::Zero(1, 1)); - std::vector R(1, MatrixXd::Zero(1, 1)); - std::vector S(1, MatrixXd::Zero(1, 1)); + vector Q(2, MatrixXd::Zero(1, 1)); + vector R(1, MatrixXd::Zero(1, 1)); + vector S(1, MatrixXd::Zero(1, 1)); Q[0](0, 0) = 2.0; Q[1](0, 0) = 4.0; R[0](0, 0) = 3.0; @@ -401,27 +399,27 @@ TEST_F(TrajectoryEvaluatorTest, QuadraticCostMatchesManual) { 1e-12); // State costs with no desired trajectory // Test any mismatched dimensions throw errors. - std::vector x_wrong(3, VectorXd::Zero(1)); + vector x_wrong(3, VectorXd::Zero(1)); ASSERT_ANY_THROW( TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(x_wrong, x_des, Q)); - std::vector u_wrong(2, VectorXd::Zero(1)); + vector u_wrong(2, VectorXd::Zero(1)); ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( x, x_des, Q, u_wrong, u_des, R)); - std::vector lambda_wrong(2, VectorXd::Zero(1)); + vector lambda_wrong(2, VectorXd::Zero(1)); ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( x, x_des, Q, u, u_des, R, lambda_wrong, lambda_des, S)); - std::vector Q_wrong_size(2, MatrixXd::Zero(2, 2)); + vector Q_wrong_size(2, MatrixXd::Zero(2, 2)); ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( x, x_des, Q_wrong_size)); - std::vector R_wrong_size(1, MatrixXd::Zero(2, 2)); + vector R_wrong_size(1, MatrixXd::Zero(2, 2)); ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( x, x_des, Q, u, u_des, R_wrong_size)); - std::vector S_wrong_size(1, MatrixXd::Zero(2, 2)); + vector S_wrong_size(1, MatrixXd::Zero(2, 2)); ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( x, x_des, Q, u, u_des, R, lambda, lambda_des, S_wrong_size)); } @@ -448,8 +446,8 @@ TEST_F(TrajectoryEvaluatorTest, SimulatePDControlWithLCSTest) { LCS lcs(A, B, D, d, E, F, H, c, N, dt); // Create a planned trajectory - std::vector x_plan(N + 1, VectorXd::Zero(n)); - std::vector u_plan(N, VectorXd::Zero(k)); + vector x_plan(N + 1, VectorXd::Zero(n)); + vector u_plan(N, VectorXd::Zero(k)); x_plan[0] << 0.0, 0.0, 0.0, 0.0; x_plan[1] << 0.5, 0.5, 0.0, 0.0; @@ -469,30 +467,65 @@ TEST_F(TrajectoryEvaluatorTest, SimulatePDControlWithLCSTest) { Kd(3) = 1.0; // Simulate with PD control - std::pair, std::vector> result = + std::pair, vector> result = TrajectoryEvaluator::SimulatePDControlWithLCS(x_plan, u_plan, Kp, Kd, lcs); - std::vector x_sim = result.first; - std::vector u_sim = result.second; + vector x_sim = result.first; + vector u_sim = result.second; EXPECT_EQ(x_sim.size(), x_plan.size()); EXPECT_EQ(u_sim.size(), u_plan.size()); EXPECT_TRUE(x_sim[0].isApprox(x_plan[0])); + // Test new overload with no feedforward argument. This should match using an + // explicit zero feedforward trajectory with use_feedforward = false. + std::pair, vector> result_no_ff = + TrajectoryEvaluator::SimulatePDControlWithLCS(x_plan, Kp, Kd, lcs); + std::pair, vector> result_zero_ff = + TrajectoryEvaluator::SimulatePDControlWithLCS( + x_plan, vector(N, VectorXd::Zero(k)), Kp, Kd, lcs, false); + EXPECT_EQ(result_no_ff.first.size(), result_zero_ff.first.size()); + EXPECT_EQ(result_no_ff.second.size(), result_zero_ff.second.size()); + for (int i = 0; i < N + 1; ++i) { + EXPECT_TRUE(result_no_ff.first[i].isApprox(result_zero_ff.first[i])); + } + for (int i = 0; i < N; ++i) { + EXPECT_TRUE(result_no_ff.second[i].isApprox(result_zero_ff.second[i])); + } + // Test with coarse and fine LCS (fine has 2x time resolution) LCS fine_lcs(A, B / 2.0, D, d, E, F, H, c, N * 2, dt / 2); - std::pair, std::vector> result_fine = + std::pair, vector> result_fine = TrajectoryEvaluator::SimulatePDControlWithLCS(x_plan, u_plan, Kp, Kd, lcs, fine_lcs); - std::vector x_sim_fine = result_fine.first; - std::vector u_sim_fine = result_fine.second; + vector x_sim_fine = result_fine.first; + vector u_sim_fine = result_fine.second; EXPECT_EQ(x_sim_fine.size(), x_plan.size()); EXPECT_EQ(u_sim_fine.size(), u_plan.size()); EXPECT_TRUE(x_sim_fine[0].isApprox(x_plan[0])); + // Test new coarse/fine overload with no feedforward argument. + std::pair, vector> result_fine_no_ff = + TrajectoryEvaluator::SimulatePDControlWithLCS(x_plan, Kp, Kd, lcs, + fine_lcs); + std::pair, vector> result_fine_zero_ff = + TrajectoryEvaluator::SimulatePDControlWithLCS( + x_plan, vector(N, VectorXd::Zero(k)), Kp, Kd, lcs, fine_lcs, + false); + EXPECT_EQ(result_fine_no_ff.first.size(), result_fine_zero_ff.first.size()); + EXPECT_EQ(result_fine_no_ff.second.size(), result_fine_zero_ff.second.size()); + for (int i = 0; i < N + 1; ++i) { + EXPECT_TRUE( + result_fine_no_ff.first[i].isApprox(result_fine_zero_ff.first[i])); + } + for (int i = 0; i < N; ++i) { + EXPECT_TRUE( + result_fine_no_ff.second[i].isApprox(result_fine_zero_ff.second[i])); + } + // Test error checking for mismatched dimensions VectorXd Kp_wrong_size = VectorXd::Zero(n + 1); ASSERT_ANY_THROW(TrajectoryEvaluator::SimulatePDControlWithLCS( @@ -510,12 +543,12 @@ TEST_F(TrajectoryEvaluatorTest, SimulatePDControlWithLCSTest) { } TEST_F(TrajectoryEvaluatorTest, ZeroOrderHoldAndDownsampleRoundTrip) { - std::vector coarse(2, VectorXd::Zero(2)); + vector coarse(2, VectorXd::Zero(2)); coarse[0] << 1.0, 2.0; coarse[1] << 3.0, 4.0; const int upsample_rate = 3; - std::vector fine = + vector fine = TrajectoryEvaluator::ZeroOrderHoldTrajectory(coarse, upsample_rate); EXPECT_EQ(fine.size(), coarse.size() * upsample_rate); for (int i = 0; i < upsample_rate; ++i) { @@ -523,7 +556,7 @@ TEST_F(TrajectoryEvaluatorTest, ZeroOrderHoldAndDownsampleRoundTrip) { EXPECT_TRUE(fine[i + upsample_rate].isApprox(coarse[1])); } - std::vector downsampled = + vector downsampled = TrajectoryEvaluator::DownsampleTrajectory(fine, upsample_rate); EXPECT_EQ(downsampled.size(), coarse.size()); EXPECT_TRUE(downsampled[0].isApprox(coarse[0])); @@ -531,20 +564,20 @@ TEST_F(TrajectoryEvaluatorTest, ZeroOrderHoldAndDownsampleRoundTrip) { } TEST_F(TrajectoryEvaluatorTest, MultiZeroOrderHoldAndDownsampleRoundTrip) { - std::vector x_coarse(3, VectorXd::Zero(2)); + vector x_coarse(3, VectorXd::Zero(2)); x_coarse[0] << 1.0, 2.0; x_coarse[1] << 3.0, 4.0; x_coarse[2] << 5.0, 6.0; - std::vector u_coarse(2, VectorXd::Zero(1)); + vector u_coarse(2, VectorXd::Zero(1)); u_coarse[0] << 7.0; u_coarse[1] << 8.0; const int upsample_rate = 3; - std::pair, std::vector> fine = + std::pair, vector> fine = TrajectoryEvaluator::ZeroOrderHoldTrajectories(x_coarse, u_coarse, upsample_rate); - std::vector x_fine = fine.first; - std::vector u_fine = fine.second; + vector x_fine = fine.first; + vector u_fine = fine.second; EXPECT_EQ(x_fine.size(), (x_coarse.size() - 1) * upsample_rate + 1); EXPECT_EQ(u_fine.size(), u_coarse.size() * upsample_rate); for (int i = 0; i < upsample_rate; ++i) { @@ -555,11 +588,11 @@ TEST_F(TrajectoryEvaluatorTest, MultiZeroOrderHoldAndDownsampleRoundTrip) { } EXPECT_TRUE(x_fine.back().isApprox(x_coarse.back())); - std::pair, std::vector> downsampled = + std::pair, vector> downsampled = TrajectoryEvaluator::DownsampleTrajectories(x_fine, u_fine, upsample_rate); - std::vector x_downsampled = downsampled.first; - std::vector u_downsampled = downsampled.second; + vector x_downsampled = downsampled.first; + vector u_downsampled = downsampled.second; EXPECT_EQ(x_downsampled.size(), 3); EXPECT_EQ(x_downsampled.size(), x_coarse.size()); EXPECT_EQ(u_downsampled.size(), 2); @@ -571,16 +604,16 @@ TEST_F(TrajectoryEvaluatorTest, MultiZeroOrderHoldAndDownsampleRoundTrip) { EXPECT_TRUE(u_downsampled[1].isApprox(u_coarse[1])); // Test any mismatched dimensions throw errors. - std::vector x_coarse_wrong(2, VectorXd::Zero(3)); + vector x_coarse_wrong(2, VectorXd::Zero(3)); ASSERT_ANY_THROW(TrajectoryEvaluator::ZeroOrderHoldTrajectories( x_coarse_wrong, u_coarse, upsample_rate)); - std::vector u_coarse_wrong(3, VectorXd::Zero(2)); + vector u_coarse_wrong(3, VectorXd::Zero(2)); ASSERT_ANY_THROW(TrajectoryEvaluator::ZeroOrderHoldTrajectories( x_coarse, u_coarse_wrong, upsample_rate)); - std::vector x_fine_wrong(2, VectorXd::Zero(3)); + vector x_fine_wrong(2, VectorXd::Zero(3)); ASSERT_ANY_THROW(TrajectoryEvaluator::DownsampleTrajectories( x_fine_wrong, u_fine, upsample_rate)); - std::vector u_fine_wrong(3, VectorXd::Zero(2)); + vector u_fine_wrong(3, VectorXd::Zero(2)); ASSERT_ANY_THROW(TrajectoryEvaluator::DownsampleTrajectories( x_fine, u_fine_wrong, upsample_rate)); } @@ -606,11 +639,11 @@ TEST_F(TrajectoryEvaluatorTest, VectorXd x_init = VectorXd::Zero(n); x_init << 1.0, 1.0; - std::vector u_plan(N, VectorXd::Zero(k)); + vector u_plan(N, VectorXd::Zero(k)); u_plan[0] << 1.0, 0.0; u_plan[1] << 0.0, 2.0; - std::vector x_sim = + vector x_sim = TrajectoryEvaluator::SimulateLCSOverTrajectory(x_init, u_plan, lcs); ASSERT_EQ(x_sim.size(), static_cast(N + 1)); @@ -622,7 +655,7 @@ TEST_F(TrajectoryEvaluatorTest, // Test with a finer LCS (smaller dt) LCS finer_lcs(A, B / 10.0, D, d, E, F, H, c, N * 10, dt / 10); - std::vector x_sim_from_finer = + vector x_sim_from_finer = TrajectoryEvaluator::SimulateLCSOverTrajectory(x_init, u_plan, lcs, finer_lcs); ASSERT_EQ(x_sim_from_finer.size(), static_cast(N + 1)); @@ -631,7 +664,7 @@ TEST_F(TrajectoryEvaluatorTest, EXPECT_TRUE(x_sim_from_finer[2].isApprox(x2_expected)); // Test any mismatched dimensions throw errors. - std::vector u_plan_wrong(2, VectorXd::Zero(3)); + vector u_plan_wrong(2, VectorXd::Zero(3)); ASSERT_ANY_THROW(TrajectoryEvaluator::SimulateLCSOverTrajectory( x_init, u_plan_wrong, lcs)); VectorXd x_init_wrong = VectorXd::Zero(3); @@ -657,8 +690,8 @@ TYPED_TEST_SUITE(C3CartpoleTypedTest, projection_types); TYPED_TEST(C3CartpoleTypedTest, UpdateLCSTest) { c3::C3* pOpt = this->pOpt.get(); auto dt = this->dt; - std::vector - pre_dynamic_constraints = pOpt->GetDynamicConstraints(); + vector pre_dynamic_constraints = + pOpt->GetDynamicConstraints(); auto& N = this->N; auto n = this->n; auto k = this->k; @@ -684,8 +717,8 @@ TYPED_TEST(C3CartpoleTypedTest, UpdateLCSTest) { pOpt->UpdateLCS(TestSystem); - std::vector - pst_dynamic_constraints = pOpt->GetDynamicConstraints(); + vector pst_dynamic_constraints = + pOpt->GetDynamicConstraints(); for (int i = 0; i < N; ++i) { // Linear Equality A matrix should be updated MatrixXd pst_Al = pst_dynamic_constraints[i]->GetDenseA(); @@ -699,8 +732,8 @@ TYPED_TEST(C3CartpoleTypedTest, End2EndCartpoleTest) { int timesteps = 1000; // number of timesteps for the simulation /// create state and input arrays - std::vector x(timesteps, VectorXd::Zero(this->n)); - std::vector input(timesteps, VectorXd::Zero(this->k)); + vector x(timesteps, VectorXd::Zero(this->n)); + vector input(timesteps, VectorXd::Zero(this->k)); x[0] = this->x0; @@ -731,9 +764,9 @@ TYPED_TEST(C3CartpoleTypedTest, ComputeCost) { // Solve one iteration of the problem. optimizer.Solve(this->x0); - std::vector x_sol = optimizer.GetStateSolution(); - std::vector u_sol = optimizer.GetInputSolution(); - std::vector lambda_sol = optimizer.GetForceSolution(); + vector x_sol = optimizer.GetStateSolution(); + vector u_sol = optimizer.GetInputSolution(); + vector lambda_sol = optimizer.GetForceSolution(); // The state trajectory excludes the N+1 time step. Add it to the end via // LCS rollout since it contributes to the C3 internal cost optimization. @@ -747,9 +780,9 @@ TYPED_TEST(C3CartpoleTypedTest, ComputeCost) { // Get the cost matrices and desired state. const C3::CostMatrices cost_matrices = optimizer.GetCostMatrices(); - std::vector Q = cost_matrices.Q; - std::vector R = cost_matrices.R; - std::vector x_des = optimizer.GetDesiredState(); + vector Q = cost_matrices.Q; + vector R = cost_matrices.R; + vector x_des = optimizer.GetDesiredState(); ASSERT_EQ(x_sol.size(), x_des.size()); ASSERT_EQ(x_sol.size(), Q.size()); ASSERT_EQ(u_sol.size(), R.size()); diff --git a/core/traj_eval.cc b/core/traj_eval.cc index 1bd8f59e..82674083 100644 --- a/core/traj_eval.cc +++ b/core/traj_eval.cc @@ -62,6 +62,19 @@ TrajectoryEvaluator::SimulatePDControlWithLCS( return std::make_pair(x, u); } +std::pair, vector> +TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, + const VectorXd& Kp, + const VectorXd& Kd, + const LCS& lcs, + const LCSSimulateConfig& config) { + const vector empty_u_plan(lcs.N(), + VectorXd::Zero(lcs.num_inputs())); + const bool use_feedforward = false; + return SimulatePDControlWithLCS(x_plan, empty_u_plan, Kp, Kd, lcs, + use_feedforward, config); +} + std::pair, vector> TrajectoryEvaluator::SimulatePDControlWithLCS( const vector& x_plan, const vector& u_plan, @@ -87,6 +100,20 @@ TrajectoryEvaluator::SimulatePDControlWithLCS( return std::make_pair(x_sim, u_sim); } +std::pair, vector> +TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, + const VectorXd& Kp, + const VectorXd& Kd, + const LCS& coarse_lcs, + const LCS& fine_lcs, + const LCSSimulateConfig& config) { + const vector empty_u_plan(coarse_lcs.N(), + VectorXd::Zero(coarse_lcs.num_inputs())); + const bool use_feedforward = false; + return SimulatePDControlWithLCS(x_plan, empty_u_plan, Kp, Kd, coarse_lcs, + fine_lcs, use_feedforward, config); +} + vector TrajectoryEvaluator::SimulateLCSOverTrajectory( const VectorXd& x_init, const vector& u_plan, const LCS& lcs, const LCSSimulateConfig& config) { diff --git a/core/traj_eval.h b/core/traj_eval.h index ce05a276..e49e07b0 100644 --- a/core/traj_eval.h +++ b/core/traj_eval.h @@ -216,6 +216,15 @@ class TrajectoryEvaluator { const Eigen::VectorXd& Kd, const LCS& lcs, const bool& use_feedforward = true, const LCSSimulateConfig& config = LCSSimulateConfig()); + /** + * @brief Special case: the input plan is not provided, so use no feedforward + * control from the plan. + */ + static std::pair, std::vector> + SimulatePDControlWithLCS( + const std::vector& x_plan, const Eigen::VectorXd& Kp, + const Eigen::VectorXd& Kd, const LCS& lcs, + const LCSSimulateConfig& config = LCSSimulateConfig()); /** * @brief Special case: simulate plans from a coarser LCS with a finer LCS. * The returned trajectory is downsampled back to be compatible with the @@ -229,6 +238,15 @@ class TrajectoryEvaluator { const Eigen::VectorXd& Kd, const LCS& coarse_lcs, const LCS& fine_lcs, const bool& use_feedforward = true, const LCSSimulateConfig& config = LCSSimulateConfig()); + /** + * *@brief Special case: simulate plans from a coarser LCS with a finer LCS, + * and the input plan is not provided. + */ + static std::pair, std::vector> + SimulatePDControlWithLCS( + const std::vector& x_plan, const Eigen::VectorXd& Kp, + const Eigen::VectorXd& Kd, const LCS& coarse_lcs, const LCS& fine_lcs, + const LCSSimulateConfig& config = LCSSimulateConfig()); /** * @brief Simulate an LCS forward from an initial condition over a trajectory From f96553a7c0f7d43cc53e44a6515995945a6559b5 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Thu, 23 Apr 2026 12:05:38 -0400 Subject: [PATCH 16/17] Evaluate quadratic trajectory cost over a subset of state vector --- bindings/pyc3/test/test_traj_eval.py | 104 +++++++++++++++++++ bindings/pyc3/traj_eval_py.cc | 100 +++++++++++++++++- core/test/core_test.cc | 65 ++++++++++++ core/traj_eval.h | 146 +++++++++++++++++++-------- 4 files changed, 370 insertions(+), 45 deletions(-) diff --git a/bindings/pyc3/test/test_traj_eval.py b/bindings/pyc3/test/test_traj_eval.py index e0254943..16a49c76 100644 --- a/bindings/pyc3/test/test_traj_eval.py +++ b/bindings/pyc3/test/test_traj_eval.py @@ -90,6 +90,110 @@ def test_compute_quadratic_trajectory_cost_zero_desired(self): ) self.assertGreater(cost, 0) + def test_compute_quadratic_trajectory_cost_with_start_end_indices(self): + data = [ + np.array([1.0, 2.0, 3.0, 4.0]), + np.array([2.0, 3.0, 4.0, 5.0]), + ] + data_des = [ + np.array([0.0, 1.0, 1.0, 1.0]), + np.array([0.0, 1.0, 1.0, 1.0]), + ] + + # Use diagonal weights so expected subset cost is easy to verify. + Q0 = np.diag([1.0, 2.0, 3.0, 4.0]) + Q1 = np.diag([5.0, 6.0, 7.0, 8.0]) + Q_list = [Q0, Q1] + + # Compute cost only on indices [1, 3). + start_index, end_index = 1, 3 + expected_subset = ( + 2.0 * (2.0 - 1.0) ** 2 + + 3.0 * (3.0 - 1.0) ** 2 + + 6.0 * (3.0 - 1.0) ** 2 + + 7.0 * (4.0 - 1.0) ** 2 + ) + + cost_list = ( + traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + start_index, + end_index, + data, + data_des, + Q_list, + ) + ) + self.assertAlmostEqual(cost_list, expected_subset) + + # Single-matrix overload should match manually computed value. + Q_single = np.diag([10.0, 20.0, 30.0, 40.0]) + expected_single = ( + 20.0 * (2.0 - 1.0) ** 2 + + 30.0 * (3.0 - 1.0) ** 2 + + 20.0 * (3.0 - 1.0) ** 2 + + 30.0 * (4.0 - 1.0) ** 2 + ) + cost_single = ( + traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + start_index, + end_index, + data, + data_des, + Q_single, + ) + ) + self.assertAlmostEqual(cost_single, expected_single) + + # Zero-desired overload with indices. + expected_zero_des = ( + 20.0 * (2.0) ** 2 + + 30.0 * (3.0) ** 2 + + 20.0 * (3.0) ** 2 + + 30.0 * (4.0) ** 2 + ) + cost_zero_des = ( + traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + start_index, + end_index, + data, + Q_single, + ) + ) + self.assertAlmostEqual(cost_zero_des, expected_zero_des) + + def test_compute_quadratic_trajectory_cost_with_start_end_index_errors( + self, + ): + data = [np.ones(self.n_x) for _ in range(self.N + 1)] + data_des = [np.zeros(self.n_x) for _ in range(self.N + 1)] + + with self.assertRaises(Exception): + traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + -1, + 2, + data, + data_des, + self.Q_matrix, + ) + + with self.assertRaises(Exception): + traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + 3, + 2, + data, + data_des, + self.Q_matrix, + ) + + with self.assertRaises(Exception): + traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + 0, + self.n_x + 1, + data, + data_des, + self.Q_matrix, + ) + def test_compute_quadratic_trajectory_cost_with_c3(self): # Create a C3 solver similar to core_test.cc but with penalize_input_change = False opts = c3.C3Options() diff --git a/bindings/pyc3/traj_eval_py.cc b/bindings/pyc3/traj_eval_py.cc index ae3b826b..2dc2abca 100644 --- a/bindings/pyc3/traj_eval_py.cc +++ b/bindings/pyc3/traj_eval_py.cc @@ -45,6 +45,21 @@ PYBIND11_MODULE(traj_eval, m) { py::arg("data"), py::arg("data_des"), py::arg("cost_matrix"), "Compute quadratic cost with single desired vector and single cost " "matrix.") + // Binding: (int, int, vector, VectorXd, MatrixXd) + .def_static( + "ComputeQuadraticTrajectoryCost", + [](const int& start_index, const int& end_index, + const std::vector& data, + const Eigen::VectorXd& data_des, + const Eigen::MatrixXd& cost_matrix) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(start_index, end_index, data, + data_des, cost_matrix); + }, + py::arg("start_index"), py::arg("end_index"), py::arg("data"), + py::arg("data_des"), py::arg("cost_matrix"), + "Compute quadratic cost with single desired vector, single cost " + "matrix, and start/end indices.") // Binding: (vector, vector, MatrixXd) .def_static( "ComputeQuadraticTrajectoryCost", @@ -56,6 +71,21 @@ PYBIND11_MODULE(traj_eval, m) { }, py::arg("data"), py::arg("data_des"), py::arg("cost_matrix"), "Compute quadratic cost with single cost matrix.") + // Binding: (int, int, vector, vector, MatrixXd) + .def_static( + "ComputeQuadraticTrajectoryCost", + [](const int& start_index, const int& end_index, + const std::vector& data, + const std::vector& data_des, + const Eigen::MatrixXd& cost_matrix) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(start_index, end_index, data, + data_des, cost_matrix); + }, + py::arg("start_index"), py::arg("end_index"), py::arg("data"), + py::arg("data_des"), py::arg("cost_matrix"), + "Compute quadratic cost with single cost matrix and start/end " + "indices.") // Binding: (vector, MatrixXd) .def_static( "ComputeQuadraticTrajectoryCost", @@ -66,6 +96,20 @@ PYBIND11_MODULE(traj_eval, m) { }, py::arg("data"), py::arg("cost_matrix"), "Compute quadratic cost with single matrix and zero desired.") + // Binding: (int, int, vector, MatrixXd) + .def_static( + "ComputeQuadraticTrajectoryCost", + [](const int& start_index, const int& end_index, + const std::vector& data, + const Eigen::MatrixXd& cost_matrix) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(start_index, end_index, data, + cost_matrix); + }, + py::arg("start_index"), py::arg("end_index"), py::arg("data"), + py::arg("cost_matrix"), + "Compute quadratic cost with single matrix, zero desired, and " + "start/end indices.") // Vector-of-matrices overloads registered AFTER single-matrix overloads. @@ -80,6 +124,21 @@ PYBIND11_MODULE(traj_eval, m) { }, py::arg("data"), py::arg("data_des"), py::arg("cost_matrices"), "Compute quadratic cost with single desired vector.") + // Binding: (int, int, vector, VectorXd, vector) + .def_static( + "ComputeQuadraticTrajectoryCost", + [](const int& start_index, const int& end_index, + const std::vector& data, + const Eigen::VectorXd& data_des, + const std::vector& cost_matrices) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(start_index, end_index, data, + data_des, cost_matrices); + }, + py::arg("start_index"), py::arg("end_index"), py::arg("data"), + py::arg("data_des"), py::arg("cost_matrices"), + "Compute quadratic cost with single desired vector and start/end " + "indices.") // Binding: (vector, vector, vector) .def_static( "ComputeQuadraticTrajectoryCost", @@ -91,6 +150,22 @@ PYBIND11_MODULE(traj_eval, m) { }, py::arg("data"), py::arg("data_des"), py::arg("cost_matrices"), "Compute quadratic cost for full trajectory cost computation.") + // Binding: (int, int, vector, vector, + // vector) + .def_static( + "ComputeQuadraticTrajectoryCost", + [](const int& start_index, const int& end_index, + const std::vector& data, + const std::vector& data_des, + const std::vector& cost_matrices) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(start_index, end_index, data, + data_des, cost_matrices); + }, + py::arg("start_index"), py::arg("end_index"), py::arg("data"), + py::arg("data_des"), py::arg("cost_matrices"), + "Compute quadratic cost for full trajectory cost computation with " + "start/end indices.") // Binding: (vector, vector) .def_static( "ComputeQuadraticTrajectoryCost", @@ -101,6 +176,20 @@ PYBIND11_MODULE(traj_eval, m) { }, py::arg("data"), py::arg("cost_matrices"), "Compute quadratic cost assuming zero desired data.") + // Binding: (int, int, vector, vector) + .def_static( + "ComputeQuadraticTrajectoryCost", + [](const int& start_index, const int& end_index, + const std::vector& data, + const std::vector& cost_matrices) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(start_index, end_index, data, + cost_matrices); + }, + py::arg("start_index"), py::arg("end_index"), py::arg("data"), + py::arg("cost_matrices"), + "Compute quadratic cost assuming zero desired data and with " + "start/end indices.") // C3-based overloads: single-matrix overloads first. @@ -174,12 +263,13 @@ PYBIND11_MODULE(traj_eval, m) { .def_static( "SimulatePDControlWithLCS", static_cast, - std::vector> (*) ( + std::vector> (*)( const std::vector&, const Eigen::VectorXd&, - const Eigen::VectorXd&, const LCS&, const c3::LCSSimulateConfig&)>( + const Eigen::VectorXd&, const LCS&, + const c3::LCSSimulateConfig&)>( &traj_eval::TrajectoryEvaluator::SimulatePDControlWithLCS), - py::arg("x_plan"), py::arg("Kp"), py::arg("Kd"), - py::arg("lcs"), py::arg("config") = c3::LCSSimulateConfig(), + py::arg("x_plan"), py::arg("Kp"), py::arg("Kd"), py::arg("lcs"), + py::arg("config") = c3::LCSSimulateConfig(), "Simulate PD control with an LCS without feedforward") // Binding: (vector, vector, VectorXd, VectorXd, // LCS, LCS, bool, LCSSimulateConfig) @@ -202,7 +292,7 @@ PYBIND11_MODULE(traj_eval, m) { .def_static( "SimulatePDControlWithLCS", static_cast, - std::vector> (*) ( + std::vector> (*)( const std::vector&, const Eigen::VectorXd&, const Eigen::VectorXd&, const LCS&, const LCS&, const c3::LCSSimulateConfig&)>( diff --git a/core/test/core_test.cc b/core/test/core_test.cc index df732ccf..72611237 100644 --- a/core/test/core_test.cc +++ b/core/test/core_test.cc @@ -425,6 +425,71 @@ TEST_F(TrajectoryEvaluatorTest, QuadraticCostMatchesManual) { x, x_des, Q, u, u_des, R, lambda, lambda_des, S_wrong_size)); } +TEST_F(TrajectoryEvaluatorTest, QuadraticCostWithStartEndIndexArguments) { + vector x(2, VectorXd::Zero(3)); + vector x_des(2, VectorXd::Zero(3)); + + x[0] << 1.0, 2.0, 3.0; + x[1] << 4.0, 5.0, 6.0; + x_des[0] << 0.5, 1.5, 1.0; + x_des[1] << 3.0, 4.5, 7.0; + + vector Q(2, MatrixXd::Zero(3, 3)); + Q[0](0, 0) = 1.0; + Q[0](1, 1) = 2.0; + Q[0](2, 2) = 3.0; + Q[1](0, 0) = 4.0; + Q[1](1, 1) = 5.0; + Q[1](2, 2) = 6.0; + + // Compute only over indices [1, 3). + const int start = 1; + const int end = 3; + double expected_subset = 0.0; + expected_subset += 2.0 * (2.0 - 1.5) * (2.0 - 1.5); + expected_subset += 3.0 * (3.0 - 1.0) * (3.0 - 1.0); + expected_subset += 5.0 * (5.0 - 4.5) * (5.0 - 4.5); + expected_subset += 6.0 * (6.0 - 7.0) * (6.0 - 7.0); + + double actual_subset = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + start, end, x, x_des, Q); + EXPECT_NEAR(actual_subset, expected_subset, 1e-12); + + // Same subset computation, but with no desired trajectory (assumed zero). + double expected_subset_zero_des = 0.0; + expected_subset_zero_des += 2.0 * 2.0 * 2.0; + expected_subset_zero_des += 3.0 * 3.0 * 3.0; + expected_subset_zero_des += 5.0 * 5.0 * 5.0; + expected_subset_zero_des += 6.0 * 6.0 * 6.0; + + double actual_subset_zero_des = + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(start, end, x, Q); + EXPECT_NEAR(actual_subset_zero_des, expected_subset_zero_des, 1e-12); + + // Repeated-cost-matrix overload with start/end. + MatrixXd Q_repeat = MatrixXd::Zero(3, 3); + Q_repeat(0, 0) = 2.0; + Q_repeat(1, 1) = 3.0; + Q_repeat(2, 2) = 4.0; + double expected_repeat_subset = 0.0; + expected_repeat_subset += 3.0 * (2.0 - 1.5) * (2.0 - 1.5); + expected_repeat_subset += 4.0 * (3.0 - 1.0) * (3.0 - 1.0); + expected_repeat_subset += 3.0 * (5.0 - 4.5) * (5.0 - 4.5); + expected_repeat_subset += 4.0 * (6.0 - 7.0) * (6.0 - 7.0); + double actual_repeat_subset = + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(start, end, x, x_des, + Q_repeat); + EXPECT_NEAR(actual_repeat_subset, expected_repeat_subset, 1e-12); + + // Invalid ranges should throw. + ASSERT_ANY_THROW( + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(-1, 2, x, x_des, Q)); + ASSERT_ANY_THROW( + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(2, 1, x, x_des, Q)); + ASSERT_ANY_THROW( + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(0, 4, x, x_des, Q)); +} + TEST_F(TrajectoryEvaluatorTest, SimulatePDControlWithLCSTest) { const int n = 4; // 4 states: 2 positions, 2 velocities const int k = 2; // 2 inputs diff --git a/core/traj_eval.h b/core/traj_eval.h index e49e07b0..64fc51c6 100644 --- a/core/traj_eval.h +++ b/core/traj_eval.h @@ -38,8 +38,14 @@ class TrajectoryEvaluator { * each individual cost is computed as: * * Cost = Sum_{i = 0, ... N-1} - * (data_i-data_des_i)^T CostMatrix_i (data_i-data_des_i) + * (data_i[start_index:end_index]-data_des_i[start_index:end_index])^T + * CostMatrix_i[start_index:end_index, start_index:end_index] + * (data_i[start_index:end_index]-data_des_i[start_index:end_index]) * + * @param start_index Starting index of the data vectors over which to compute + * costs (inclusive) + * @param end_index Ending index of the data vectors over which to compute + * costs (exclusive) * @param data Trajectory of length N of vectors * @param data_des Trajectory of length N of desired vectors * @param cost_matrices Trajectory of length N of cost matrices. This can be @@ -60,49 +66,29 @@ class TrajectoryEvaluator { typename = std::enable_if_t::value && detail::IsCostType::value>> static double ComputeQuadraticTrajectoryCost( + const int& start_index, const int& end_index, const std::vector& data, const T_des& data_des, const T_cost& cost_matrices, Rest&&... rest) { - double cost = 0.0; - int N = data.size(); - int n_data = data[0].size(); - - // Handle if data_des is provided as a single vector instead of a trajectory - // of vectors. - std::vector data_des_vec; - if constexpr (std::is_same_v>) { - DRAKE_THROW_UNLESS(N == data_des.size()); - data_des_vec = data_des; - } else { - bool is_single_vector = std::is_same_v; - DRAKE_THROW_UNLESS(is_single_vector); - data_des_vec = std::vector(N, data_des); - } - - // Handle if cost_matrices is provided as a single matrix instead of a - // trajectory of matrices. - std::vector cost_matrices_vec; - if constexpr (std::is_same_v>) { - DRAKE_THROW_UNLESS(N == cost_matrices.size()); - cost_matrices_vec = cost_matrices; - } else { - bool is_single_matrix = std::is_same_v; - DRAKE_THROW_UNLESS(is_single_matrix); - cost_matrices_vec = std::vector(N, cost_matrices); - } - - // Sum the costs and check sizes. - for (int i = 0; i < N; i++) { - DRAKE_THROW_UNLESS(data[i].size() == n_data); - DRAKE_THROW_UNLESS(data_des_vec[i].size() == n_data); - DRAKE_THROW_UNLESS(cost_matrices_vec[i].rows() == n_data); - DRAKE_THROW_UNLESS(cost_matrices_vec[i].cols() == n_data); - - cost += (data[i] - data_des_vec[i]).transpose() * cost_matrices_vec[i] * - (data[i] - data_des_vec[i]); - } + return ComputeQuadraticTrajectoryCostPrivate(start_index, end_index, data, + data_des, cost_matrices, + std::forward(rest)...); + } - return cost + ComputeQuadraticTrajectoryCost(std::forward(rest)...); + /** @brief Same as above, but no start_index and end_index are provided, so + * the cost is computed over the entire data vectors. + */ + template ::value && + detail::IsCostType::value>> + static double ComputeQuadraticTrajectoryCost( + const std::vector& data, const T_des& data_des, + const T_cost& cost_matrices, Rest&&... rest) { + DRAKE_THROW_UNLESS(!data.empty()); + return ComputeQuadraticTrajectoryCostPrivate(0, data[0].size(), data, + data_des, cost_matrices, + std::forward(rest)...); } + /** @brief Special case: no desired data is provided, so it is assumed the * desired data is zero. */ @@ -111,12 +97,30 @@ class TrajectoryEvaluator { static double ComputeQuadraticTrajectoryCost( const std::vector& data, const T_cost& cost_matrices, Rest&&... rest) { + DRAKE_THROW_UNLESS(!data.empty()); return ComputeQuadraticTrajectoryCost( data, std::vector(data.size(), Eigen::VectorXd::Zero(data[0].size())), cost_matrices, std::forward(rest)...); } + + /** @brief Same as above but computes cost only over [start_index, + * end_index). + */ + template ::value>> + static double ComputeQuadraticTrajectoryCost( + const int& start_index, const int& end_index, + const std::vector& data, const T_cost& cost_matrices, + Rest&&... rest) { + DRAKE_THROW_UNLESS(!data.empty()); + return ComputeQuadraticTrajectoryCost( + start_index, end_index, data, + std::vector(data.size(), + Eigen::VectorXd::Zero(data[0].size())), + cost_matrices, std::forward(rest)...); + } /** @brief Special case: a C3 object is the input argument, and the cost is to * be computed based on the trajectories contained in the C3 object and the * cost matrices contained in the C3 object for state and input costs. @@ -376,6 +380,68 @@ class TrajectoryEvaluator { * not be called externally with no input arguments. */ static double ComputeQuadraticTrajectoryCost() { return 0.0; } + + /** @brief Private implementation of ComputeQuadraticTrajectoryCost for + * handling the general case with start/end index arguments. + */ + template ::value && + detail::IsCostType::value>> + static double ComputeQuadraticTrajectoryCostPrivate( + const int& start_index, const int& end_index, + const std::vector& data, const T_des& data_des, + const T_cost& cost_matrices, Rest&&... rest) { + double cost = 0.0; + int N = data.size(); + DRAKE_THROW_UNLESS(N > 0); + int n_data = data[0].size(); + + DRAKE_THROW_UNLESS(start_index >= 0); + DRAKE_THROW_UNLESS(end_index >= start_index); + DRAKE_THROW_UNLESS(end_index <= n_data); + int subset_size = end_index - start_index; + + // Handle if data_des is provided as a single vector instead of a trajectory + // of vectors. + std::vector data_des_vec; + if constexpr (std::is_same_v>) { + DRAKE_THROW_UNLESS(N == data_des.size()); + data_des_vec = data_des; + } else { + bool is_single_vector = std::is_same_v; + DRAKE_THROW_UNLESS(is_single_vector); + data_des_vec = std::vector(N, data_des); + } + + // Handle if cost_matrices is provided as a single matrix instead of a + // trajectory of matrices. + std::vector cost_matrices_vec; + if constexpr (std::is_same_v>) { + DRAKE_THROW_UNLESS(N == cost_matrices.size()); + cost_matrices_vec = cost_matrices; + } else { + bool is_single_matrix = std::is_same_v; + DRAKE_THROW_UNLESS(is_single_matrix); + cost_matrices_vec = std::vector(N, cost_matrices); + } + + // Sum the costs and check sizes. + for (int i = 0; i < N; i++) { + DRAKE_THROW_UNLESS(data[i].size() == n_data); + DRAKE_THROW_UNLESS(data_des_vec[i].size() == n_data); + DRAKE_THROW_UNLESS(cost_matrices_vec[i].rows() == n_data); + DRAKE_THROW_UNLESS(cost_matrices_vec[i].cols() == n_data); + + cost += (data[i] - data_des_vec[i]) + .segment(start_index, subset_size) + .transpose() * + cost_matrices_vec[i].block(start_index, start_index, subset_size, + subset_size) * + (data[i] - data_des_vec[i]).segment(start_index, subset_size); + } + + return cost + ComputeQuadraticTrajectoryCost(std::forward(rest)...); + } }; } // namespace traj_eval From d95bdf834be3b031ac3d7772dfdacda97723dd5a Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Thu, 23 Apr 2026 14:21:07 -0400 Subject: [PATCH 17/17] Remove unnecessary private implementation and move functionality into existing public version --- core/traj_eval.h | 121 +++++++++++++++++++++-------------------------- 1 file changed, 53 insertions(+), 68 deletions(-) diff --git a/core/traj_eval.h b/core/traj_eval.h index 64fc51c6..363cd392 100644 --- a/core/traj_eval.h +++ b/core/traj_eval.h @@ -69,9 +69,56 @@ class TrajectoryEvaluator { const int& start_index, const int& end_index, const std::vector& data, const T_des& data_des, const T_cost& cost_matrices, Rest&&... rest) { - return ComputeQuadraticTrajectoryCostPrivate(start_index, end_index, data, - data_des, cost_matrices, - std::forward(rest)...); + double cost = 0.0; + int N = data.size(); + DRAKE_THROW_UNLESS(N > 0); + int n_data = data[0].size(); + + DRAKE_THROW_UNLESS(start_index >= 0); + DRAKE_THROW_UNLESS(end_index >= start_index); + DRAKE_THROW_UNLESS(end_index <= n_data); + int subset_size = end_index - start_index; + + // Handle if data_des is provided as a single vector instead of a trajectory + // of vectors. + std::vector data_des_vec; + if constexpr (std::is_same_v>) { + DRAKE_THROW_UNLESS(N == data_des.size()); + data_des_vec = data_des; + } else { + bool is_single_vector = std::is_same_v; + DRAKE_THROW_UNLESS(is_single_vector); + data_des_vec = std::vector(N, data_des); + } + + // Handle if cost_matrices is provided as a single matrix instead of a + // trajectory of matrices. + std::vector cost_matrices_vec; + if constexpr (std::is_same_v>) { + DRAKE_THROW_UNLESS(N == cost_matrices.size()); + cost_matrices_vec = cost_matrices; + } else { + bool is_single_matrix = std::is_same_v; + DRAKE_THROW_UNLESS(is_single_matrix); + cost_matrices_vec = std::vector(N, cost_matrices); + } + + // Sum the costs and check sizes. + for (int i = 0; i < N; i++) { + DRAKE_THROW_UNLESS(data[i].size() == n_data); + DRAKE_THROW_UNLESS(data_des_vec[i].size() == n_data); + DRAKE_THROW_UNLESS(cost_matrices_vec[i].rows() == n_data); + DRAKE_THROW_UNLESS(cost_matrices_vec[i].cols() == n_data); + + cost += (data[i] - data_des_vec[i]) + .segment(start_index, subset_size) + .transpose() * + cost_matrices_vec[i].block(start_index, start_index, subset_size, + subset_size) * + (data[i] - data_des_vec[i]).segment(start_index, subset_size); + } + + return cost + ComputeQuadraticTrajectoryCost(std::forward(rest)...); } /** @brief Same as above, but no start_index and end_index are provided, so @@ -84,9 +131,9 @@ class TrajectoryEvaluator { const std::vector& data, const T_des& data_des, const T_cost& cost_matrices, Rest&&... rest) { DRAKE_THROW_UNLESS(!data.empty()); - return ComputeQuadraticTrajectoryCostPrivate(0, data[0].size(), data, - data_des, cost_matrices, - std::forward(rest)...); + return ComputeQuadraticTrajectoryCost(0, data[0].size(), data, data_des, + cost_matrices, + std::forward(rest)...); } /** @brief Special case: no desired data is provided, so it is assumed the @@ -380,68 +427,6 @@ class TrajectoryEvaluator { * not be called externally with no input arguments. */ static double ComputeQuadraticTrajectoryCost() { return 0.0; } - - /** @brief Private implementation of ComputeQuadraticTrajectoryCost for - * handling the general case with start/end index arguments. - */ - template ::value && - detail::IsCostType::value>> - static double ComputeQuadraticTrajectoryCostPrivate( - const int& start_index, const int& end_index, - const std::vector& data, const T_des& data_des, - const T_cost& cost_matrices, Rest&&... rest) { - double cost = 0.0; - int N = data.size(); - DRAKE_THROW_UNLESS(N > 0); - int n_data = data[0].size(); - - DRAKE_THROW_UNLESS(start_index >= 0); - DRAKE_THROW_UNLESS(end_index >= start_index); - DRAKE_THROW_UNLESS(end_index <= n_data); - int subset_size = end_index - start_index; - - // Handle if data_des is provided as a single vector instead of a trajectory - // of vectors. - std::vector data_des_vec; - if constexpr (std::is_same_v>) { - DRAKE_THROW_UNLESS(N == data_des.size()); - data_des_vec = data_des; - } else { - bool is_single_vector = std::is_same_v; - DRAKE_THROW_UNLESS(is_single_vector); - data_des_vec = std::vector(N, data_des); - } - - // Handle if cost_matrices is provided as a single matrix instead of a - // trajectory of matrices. - std::vector cost_matrices_vec; - if constexpr (std::is_same_v>) { - DRAKE_THROW_UNLESS(N == cost_matrices.size()); - cost_matrices_vec = cost_matrices; - } else { - bool is_single_matrix = std::is_same_v; - DRAKE_THROW_UNLESS(is_single_matrix); - cost_matrices_vec = std::vector(N, cost_matrices); - } - - // Sum the costs and check sizes. - for (int i = 0; i < N; i++) { - DRAKE_THROW_UNLESS(data[i].size() == n_data); - DRAKE_THROW_UNLESS(data_des_vec[i].size() == n_data); - DRAKE_THROW_UNLESS(cost_matrices_vec[i].rows() == n_data); - DRAKE_THROW_UNLESS(cost_matrices_vec[i].cols() == n_data); - - cost += (data[i] - data_des_vec[i]) - .segment(start_index, subset_size) - .transpose() * - cost_matrices_vec[i].block(start_index, start_index, subset_size, - subset_size) * - (data[i] - data_des_vec[i]).segment(start_index, subset_size); - } - - return cost + ComputeQuadraticTrajectoryCost(std::forward(rest)...); - } }; } // namespace traj_eval