From 8f0deafc79dd6f878790a89609b7d5e18a064ee4 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Mon, 23 Feb 2026 17:23:42 -0500 Subject: [PATCH 1/7] 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 f46c20a..e52ff8d 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 1fe98ff..fe4e679 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 2/7] 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 703c58f..f669e84 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 e52ff8d..e04413f 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 3/7] 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 f669e84..99ee8e2 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 fe4e679..2a08306 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 3756c0b..fcdc673 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 8de9c14..01e2a02 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 18815c4..71f55f6 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 0000000..b33977a --- /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 0000000..c0138c7 --- /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 4/7] 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 a5a6ac5..c762091 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 71f55f6..09e82b5 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 b33977a..8cec730 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 0000000..d9f093b --- /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 c0138c7..0000000 --- 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 5/7] 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 c762091..ebca23f 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 09e82b5..22d0145 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 8cec730..2608283 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 d9f093b..a22afa0 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 6/7] 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 99ee8e2..703c58f 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 152c822..8d83708 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 2a08306..498a843 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 22d0145..e07b90d 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 a22afa0..bd0273b 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 7/7] 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 e07b90d..5bfda39 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 2608283..afeddc3 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 bd0273b..7af127b 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