diff --git a/core/BUILD.bazel b/core/BUILD.bazel index ec5c92e..ebca23f 100644 --- a/core/BUILD.bazel +++ b/core/BUILD.bazel @@ -74,6 +74,16 @@ cc_library( ], ) +cc_library( + name = "traj_eval", + srcs = ["traj_eval.cc"], + hdrs = ["traj_eval.h"], + deps = [ + ":c3", + "@drake//:drake_shared_library", + ], +) + filegroup( name = 'test_data', srcs = glob(['test/resources/**']) @@ -98,6 +108,7 @@ cc_test( deps = [ ":c3", ":c3_cartpole_problem", + ":traj_eval", "@gtest//:main", ], env_inherit = [ diff --git a/core/c3.cc b/core/c3.cc index f46c20a..8d83708 100644 --- a/core/c3.cc +++ b/core/c3.cc @@ -31,10 +31,10 @@ 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) { this->Q = Q; this->R = R; this->G = G; @@ -82,19 +82,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 +183,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) { @@ -232,12 +230,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 +259,7 @@ void C3::UpdateCostMatrices(const CostMatrices& costs) { } } -const std::vector& C3::GetTargetCost() { +const vector& C3::GetTargetCost() { return target_costs_; } @@ -285,7 +283,8 @@ void C3::Solve(const VectorXd& x0) { VectorXd lambda0; LCPSolver.SolveLcpLemke(lcs_.F()[0], lcs_.E()[0] * x0 + lcs_.c()[0], &lambda0); - // Force constraints to be updated before every solve if no dependence on u + // Force constraints to be updated before every solve if no dependence on + // u if (initial_force_constraint_) { initial_force_constraint_->UpdateCoefficients( MatrixXd::Identity(n_lambda_, n_lambda_), lambda0); @@ -312,8 +311,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 +384,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 +500,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 +528,9 @@ void C3::AddLinearConstraint(const Eigen::MatrixXd& A, void C3::AddLinearConstraint(const Eigen::RowVectorXd& A, double lower_bound, double upper_bound, ConstraintVariable constraint) { - Eigen::VectorXd lb(1); + VectorXd lb(1); lb << lower_bound; - Eigen::VectorXd ub(1); + VectorXd ub(1); ub << upper_bound; AddLinearConstraint(A, lb, ub, constraint); } @@ -544,7 +542,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..498a843 100644 --- a/core/c3.h +++ b/core/c3.h @@ -50,6 +50,8 @@ class C3 { std::vector R; std::vector G; std::vector U; + std::vector Q_evaluation; + std::vector R_evaluation; }; /*! 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..5bfda39 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; @@ -43,6 +45,7 @@ using namespace c3; * | CreatePlaceholderLCS | DONE | * | WarmStartSmokeTest | DONE | * | # of regression tests | 2 | + * | 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 @@ -320,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, 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, 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 + 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, 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, 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. + 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, x_des, Q, u_wrong, u_des, R)); + + std::vector lambda_wrong(2, VectorXd::Zero(1)); + ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + x, x_des, Q, u, u_des, R, lambda_wrong, lambda_des, S)); + + std::vector Q_wrong_size(2, MatrixXd::Zero(2, 2)); + 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, 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, x_des, Q, u, u_des, R, lambda, lambda_des, 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 new file mode 100644 index 0000000..afeddc3 --- /dev/null +++ b/core/traj_eval.cc @@ -0,0 +1,232 @@ +#include "traj_eval.h" + +#include +#include + +#include + +#include "lcs.h" +#include "solver_options_io.h" + +namespace c3 { +namespace traj_eval { + +using Eigen::MatrixXd; +using Eigen::VectorXd; +using std::vector; + +std::pair, vector> +TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, + const vector& u_plan, + const VectorXd& Kp, + const VectorXd& Kd, + const LCS& lcs) { + 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_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; + 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> +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 + // 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 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); + + 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 TrajectoryEvaluator::SimulateLCSOverTrajectory( + const vector& x_plan, const vector& u_plan, + const LCS& lcs) { + return SimulateLCSOverTrajectory(x_plan[0], u_plan, 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 = + ZeroOrderHoldTrajectory(u_plan, timestep_split); + vector x_sim_fine = + SimulateLCSOverTrajectory(x_init, u_plan_finer, fine_lcs); + return DownsampleTrajectories(x_sim_fine, u_plan_finer, timestep_split).first; +} + +vector TrajectoryEvaluator::SimulateLCSOverTrajectory( + const vector& x_plan, const vector& u_plan, + const LCS& coarse_lcs, const LCS& fine_lcs) { + return SimulateLCSOverTrajectory(x_plan[0], u_plan, coarse_lcs, fine_lcs); +} + +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 + // 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> +TrajectoryEvaluator::ZeroOrderHoldTrajectories(const vector& x_coarse, + const vector& u_coarse, + const int& 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, 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> +TrajectoryEvaluator::DownsampleTrajectories(const vector& x_fine, + const vector& u_fine, + const int& 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_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_THROW_UNLESS(fine_lcs.dt() * timestep_split == coarse_lcs.dt()); + DRAKE_THROW_UNLESS(coarse_lcs.N() * timestep_split == fine_lcs.N()); + return timestep_split; +} + +void TrajectoryEvaluator::CheckLCSAndTrajectoryCompatibility( + const LCS& lcs, const std::vector& x, + const std::vector& u, + const std::vector& lambda) { + DRAKE_THROW_UNLESS(lcs.N() == x.size() - 1 && lcs.N() == u.size() && + lcs.N() == lambda.size()); + for (int i = 0; i < lcs.N() + 1; i++) { + DRAKE_THROW_UNLESS(x[i].size() == lcs.num_states()); + if (i < lcs.N()) { + DRAKE_THROW_UNLESS(u[i].size() == lcs.num_inputs()); + DRAKE_THROW_UNLESS(lambda[i].size() == lcs.num_lambdas()); + } + } +} + +void TrajectoryEvaluator::CheckLCSAndTrajectoryCompatibility( + const LCS& lcs, const std::vector& x, + const std::vector& u) { + return CheckLCSAndTrajectoryCompatibility( + lcs, x, u, vector(lcs.N(), VectorXd::Zero(lcs.num_lambdas()))); +} + +void TrajectoryEvaluator::CheckLCSAndTrajectoryCompatibility( + const LCS& lcs, const std::vector& x) { + return CheckLCSAndTrajectoryCompatibility( + lcs, x, vector(lcs.N(), VectorXd::Zero(lcs.num_inputs())), + vector(lcs.N(), VectorXd::Zero(lcs.num_lambdas()))); +} + +} // namespace traj_eval +} // namespace c3 diff --git a/core/traj_eval.h b/core/traj_eval.h new file mode 100644 index 0000000..7af127b --- /dev/null +++ b/core/traj_eval.h @@ -0,0 +1,232 @@ +#pragma once + +#include +#include + +#include + +#include "c3_options.h" +#include "lcs.h" + +#include "drake/common/drake_throw.h" + +namespace c3 { +namespace traj_eval { + +/// Utility class for trajectory evaluation computations and simulations. +class TrajectoryEvaluator { + public: + /** + * @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} + * (data_i-data_des_i)^T CostMatrix_i (data_i-data_des_i) + * + * @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& 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& 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 + * 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); + /** + * @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); + + /** + * @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); + /** @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); + /** + * @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); + /** + * @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); + + /** + * @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); + /** + * @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); + + /** + * @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); + /** @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); + + /** + * @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); + + /** + * @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); + /** @brief Special case: no lambdas provided. */ + static void CheckLCSAndTrajectoryCompatibility( + const LCS& lcs, const std::vector& x, + const std::vector& u); + /** @brief Special case: no inputs or lambdas provided. */ + static void CheckLCSAndTrajectoryCompatibility( + const LCS& lcs, const std::vector& x); +}; + +} // namespace traj_eval +} // namespace c3