From 8f0deafc79dd6f878790a89609b7d5e18a064ee4 Mon Sep 17 00:00:00 2001 From: Bibit Bianchini Date: Mon, 23 Feb 2026 17:23:42 -0500 Subject: [PATCH 1/3] 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/3] 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/3] 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