diff --git a/bindings/pyc3/test/test_traj_eval.py b/bindings/pyc3/test/test_traj_eval.py index 57886918..16a49c76 100644 --- a/bindings/pyc3/test/test_traj_eval.py +++ b/bindings/pyc3/test/test_traj_eval.py @@ -46,7 +46,9 @@ def setUp(self): self.Q_matrices = [ np.eye(self.n_x, dtype=np.float64) for _ in range(self.N + 1) ] - self.R_matrices = [np.eye(self.n_u, dtype=np.float64) for _ in range(self.N)] + self.R_matrices = [ + np.eye(self.n_u, dtype=np.float64) for _ in range(self.N) + ] def test_compute_quadratic_trajectory_cost_basic(self): # Test with single matrices @@ -88,6 +90,110 @@ def test_compute_quadratic_trajectory_cost_zero_desired(self): ) self.assertGreater(cost, 0) + def test_compute_quadratic_trajectory_cost_with_start_end_indices(self): + data = [ + np.array([1.0, 2.0, 3.0, 4.0]), + np.array([2.0, 3.0, 4.0, 5.0]), + ] + data_des = [ + np.array([0.0, 1.0, 1.0, 1.0]), + np.array([0.0, 1.0, 1.0, 1.0]), + ] + + # Use diagonal weights so expected subset cost is easy to verify. + Q0 = np.diag([1.0, 2.0, 3.0, 4.0]) + Q1 = np.diag([5.0, 6.0, 7.0, 8.0]) + Q_list = [Q0, Q1] + + # Compute cost only on indices [1, 3). + start_index, end_index = 1, 3 + expected_subset = ( + 2.0 * (2.0 - 1.0) ** 2 + + 3.0 * (3.0 - 1.0) ** 2 + + 6.0 * (3.0 - 1.0) ** 2 + + 7.0 * (4.0 - 1.0) ** 2 + ) + + cost_list = ( + traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + start_index, + end_index, + data, + data_des, + Q_list, + ) + ) + self.assertAlmostEqual(cost_list, expected_subset) + + # Single-matrix overload should match manually computed value. + Q_single = np.diag([10.0, 20.0, 30.0, 40.0]) + expected_single = ( + 20.0 * (2.0 - 1.0) ** 2 + + 30.0 * (3.0 - 1.0) ** 2 + + 20.0 * (3.0 - 1.0) ** 2 + + 30.0 * (4.0 - 1.0) ** 2 + ) + cost_single = ( + traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + start_index, + end_index, + data, + data_des, + Q_single, + ) + ) + self.assertAlmostEqual(cost_single, expected_single) + + # Zero-desired overload with indices. + expected_zero_des = ( + 20.0 * (2.0) ** 2 + + 30.0 * (3.0) ** 2 + + 20.0 * (3.0) ** 2 + + 30.0 * (4.0) ** 2 + ) + cost_zero_des = ( + traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + start_index, + end_index, + data, + Q_single, + ) + ) + self.assertAlmostEqual(cost_zero_des, expected_zero_des) + + def test_compute_quadratic_trajectory_cost_with_start_end_index_errors( + self, + ): + data = [np.ones(self.n_x) for _ in range(self.N + 1)] + data_des = [np.zeros(self.n_x) for _ in range(self.N + 1)] + + with self.assertRaises(Exception): + traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + -1, + 2, + data, + data_des, + self.Q_matrix, + ) + + with self.assertRaises(Exception): + traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + 3, + 2, + data, + data_des, + self.Q_matrix, + ) + + with self.assertRaises(Exception): + traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + 0, + self.n_x + 1, + data, + data_des, + self.Q_matrix, + ) + def test_compute_quadratic_trajectory_cost_with_c3(self): # Create a C3 solver similar to core_test.cc but with penalize_input_change = False opts = c3.C3Options() @@ -103,14 +209,18 @@ def test_compute_quadratic_trajectory_cost_with_c3(self): opts.M = 100.0 opts.gamma = 1.0 opts.rho_scale = 1.0 - opts.penalize_input_change = False # This should prevent the constraint failure + opts.penalize_input_change = ( + False # This should prevent the constraint failure + ) costs = c3.C3.CreateCostMatricesFromC3Options(opts, self.N) solver = c3.C3QP(self.lcs, costs, self.x_des, opts) solver.Solve(np.zeros(self.n_x)) # Test cost computation from C3 solver - cost = traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost(solver) + cost = traj_eval.TrajectoryEvaluator.ComputeQuadraticTrajectoryCost( + solver + ) self.assertGreaterEqual(cost, 0) # Test with custom matrices @@ -141,11 +251,49 @@ def test_simulate_pd_control_with_lcs(self): for u in u_sim: self.assertEqual(len(u), self.n_u) + def test_simulate_pd_control_with_lcs_no_feedforward_overload(self): + # Test the overload without u_plan. + Kp = np.zeros(self.n_x) + Kd = np.zeros(self.n_x) + Kp[0] = 10.0 + Kp[1] = 10.0 + Kd[2] = 1.0 + Kd[3] = 1.0 + + x_sim, u_sim = traj_eval.TrajectoryEvaluator.SimulatePDControlWithLCS( + self.x_traj, Kp, Kd, self.lcs + ) + + self.assertEqual(len(x_sim), self.N + 1) + self.assertEqual(len(u_sim), self.N) + for x in x_sim: + self.assertEqual(len(x), self.n_x) + for u in u_sim: + self.assertEqual(len(u), self.n_u) + + # This overload should match explicit zero feedforward behavior. + zero_u_plan = [np.zeros(self.n_u) for _ in range(self.N)] + x_sim_zero, u_sim_zero = ( + traj_eval.TrajectoryEvaluator.SimulatePDControlWithLCS( + self.x_traj, + zero_u_plan, + Kp, + Kd, + self.lcs, + False, + ) + ) + for x_a, x_b in zip(x_sim, x_sim_zero): + np.testing.assert_allclose(x_a, x_b) + for u_a, u_b in zip(u_sim, u_sim_zero): + np.testing.assert_allclose(u_a, u_b) + def test_simulate_pd_control_with_coarse_fine_lcs(self): # Create fine LCS with smaller dt, following core_test.cc pattern fine_lcs = c3.LCS( np.eye(self.n_x), - np.ones((self.n_x, self.n_u)) / 2.0, # Scale B matrix for finer time step + np.ones((self.n_x, self.n_u)) + / 2.0, # Scale B matrix for finer time step np.ones((self.n_x, self.n_lambda)), np.zeros(self.n_x), np.ones((self.n_lambda, self.n_x)), @@ -171,6 +319,55 @@ def test_simulate_pd_control_with_coarse_fine_lcs(self): self.assertEqual(len(x_sim), self.N + 1) self.assertEqual(len(u_sim), self.N) + def test_simulate_pd_control_with_coarse_fine_lcs_no_feedforward_overload( + self, + ): + # Test the coarse/fine overload without u_plan. + fine_lcs = c3.LCS( + np.eye(self.n_x), + np.ones((self.n_x, self.n_u)) / 2.0, + np.ones((self.n_x, self.n_lambda)), + np.zeros(self.n_x), + np.ones((self.n_lambda, self.n_x)), + np.eye(self.n_lambda), + np.ones((self.n_lambda, self.n_u)), + np.ones(self.n_lambda), + self.N * 2, + self.dt / 2, + ) + + Kp = np.zeros(self.n_x) + Kd = np.zeros(self.n_x) + Kp[0] = 10.0 + Kp[1] = 10.0 + Kd[2] = 1.0 + Kd[3] = 1.0 + + x_sim, u_sim = traj_eval.TrajectoryEvaluator.SimulatePDControlWithLCS( + self.x_traj, Kp, Kd, self.lcs, fine_lcs + ) + + self.assertEqual(len(x_sim), self.N + 1) + self.assertEqual(len(u_sim), self.N) + + # This overload should match explicit zero feedforward behavior. + zero_u_plan = [np.zeros(self.n_u) for _ in range(self.N)] + x_sim_zero, u_sim_zero = ( + traj_eval.TrajectoryEvaluator.SimulatePDControlWithLCS( + self.x_traj, + zero_u_plan, + Kp, + Kd, + self.lcs, + fine_lcs, + False, + ) + ) + for x_a, x_b in zip(x_sim, x_sim_zero): + np.testing.assert_allclose(x_a, x_b) + for u_a, u_b in zip(u_sim, u_sim_zero): + np.testing.assert_allclose(u_a, u_b) + def test_simulate_lcs_over_trajectory(self): config = c3.LCSSimulateConfig() @@ -320,7 +517,9 @@ def test_trajectory_compatibility_errors(self): ) # Test with wrong length - short_x = [np.ones(self.n_x) for _ in range(self.N)] # Missing one state + short_x = [ + np.ones(self.n_x) for _ in range(self.N) + ] # Missing one state with self.assertRaises(Exception): traj_eval.TrajectoryEvaluator.CheckLCSAndTrajectoryCompatibility( diff --git a/bindings/pyc3/traj_eval_py.cc b/bindings/pyc3/traj_eval_py.cc index 9c95070b..2dc2abca 100644 --- a/bindings/pyc3/traj_eval_py.cc +++ b/bindings/pyc3/traj_eval_py.cc @@ -45,6 +45,21 @@ PYBIND11_MODULE(traj_eval, m) { py::arg("data"), py::arg("data_des"), py::arg("cost_matrix"), "Compute quadratic cost with single desired vector and single cost " "matrix.") + // Binding: (int, int, vector, VectorXd, MatrixXd) + .def_static( + "ComputeQuadraticTrajectoryCost", + [](const int& start_index, const int& end_index, + const std::vector& data, + const Eigen::VectorXd& data_des, + const Eigen::MatrixXd& cost_matrix) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(start_index, end_index, data, + data_des, cost_matrix); + }, + py::arg("start_index"), py::arg("end_index"), py::arg("data"), + py::arg("data_des"), py::arg("cost_matrix"), + "Compute quadratic cost with single desired vector, single cost " + "matrix, and start/end indices.") // Binding: (vector, vector, MatrixXd) .def_static( "ComputeQuadraticTrajectoryCost", @@ -56,6 +71,21 @@ PYBIND11_MODULE(traj_eval, m) { }, py::arg("data"), py::arg("data_des"), py::arg("cost_matrix"), "Compute quadratic cost with single cost matrix.") + // Binding: (int, int, vector, vector, MatrixXd) + .def_static( + "ComputeQuadraticTrajectoryCost", + [](const int& start_index, const int& end_index, + const std::vector& data, + const std::vector& data_des, + const Eigen::MatrixXd& cost_matrix) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(start_index, end_index, data, + data_des, cost_matrix); + }, + py::arg("start_index"), py::arg("end_index"), py::arg("data"), + py::arg("data_des"), py::arg("cost_matrix"), + "Compute quadratic cost with single cost matrix and start/end " + "indices.") // Binding: (vector, MatrixXd) .def_static( "ComputeQuadraticTrajectoryCost", @@ -66,6 +96,20 @@ PYBIND11_MODULE(traj_eval, m) { }, py::arg("data"), py::arg("cost_matrix"), "Compute quadratic cost with single matrix and zero desired.") + // Binding: (int, int, vector, MatrixXd) + .def_static( + "ComputeQuadraticTrajectoryCost", + [](const int& start_index, const int& end_index, + const std::vector& data, + const Eigen::MatrixXd& cost_matrix) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(start_index, end_index, data, + cost_matrix); + }, + py::arg("start_index"), py::arg("end_index"), py::arg("data"), + py::arg("cost_matrix"), + "Compute quadratic cost with single matrix, zero desired, and " + "start/end indices.") // Vector-of-matrices overloads registered AFTER single-matrix overloads. @@ -80,6 +124,21 @@ PYBIND11_MODULE(traj_eval, m) { }, py::arg("data"), py::arg("data_des"), py::arg("cost_matrices"), "Compute quadratic cost with single desired vector.") + // Binding: (int, int, vector, VectorXd, vector) + .def_static( + "ComputeQuadraticTrajectoryCost", + [](const int& start_index, const int& end_index, + const std::vector& data, + const Eigen::VectorXd& data_des, + const std::vector& cost_matrices) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(start_index, end_index, data, + data_des, cost_matrices); + }, + py::arg("start_index"), py::arg("end_index"), py::arg("data"), + py::arg("data_des"), py::arg("cost_matrices"), + "Compute quadratic cost with single desired vector and start/end " + "indices.") // Binding: (vector, vector, vector) .def_static( "ComputeQuadraticTrajectoryCost", @@ -91,6 +150,22 @@ PYBIND11_MODULE(traj_eval, m) { }, py::arg("data"), py::arg("data_des"), py::arg("cost_matrices"), "Compute quadratic cost for full trajectory cost computation.") + // Binding: (int, int, vector, vector, + // vector) + .def_static( + "ComputeQuadraticTrajectoryCost", + [](const int& start_index, const int& end_index, + const std::vector& data, + const std::vector& data_des, + const std::vector& cost_matrices) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(start_index, end_index, data, + data_des, cost_matrices); + }, + py::arg("start_index"), py::arg("end_index"), py::arg("data"), + py::arg("data_des"), py::arg("cost_matrices"), + "Compute quadratic cost for full trajectory cost computation with " + "start/end indices.") // Binding: (vector, vector) .def_static( "ComputeQuadraticTrajectoryCost", @@ -101,6 +176,20 @@ PYBIND11_MODULE(traj_eval, m) { }, py::arg("data"), py::arg("cost_matrices"), "Compute quadratic cost assuming zero desired data.") + // Binding: (int, int, vector, vector) + .def_static( + "ComputeQuadraticTrajectoryCost", + [](const int& start_index, const int& end_index, + const std::vector& data, + const std::vector& cost_matrices) -> double { + return traj_eval::TrajectoryEvaluator:: + ComputeQuadraticTrajectoryCost(start_index, end_index, data, + cost_matrices); + }, + py::arg("start_index"), py::arg("end_index"), py::arg("data"), + py::arg("cost_matrices"), + "Compute quadratic cost assuming zero desired data and with " + "start/end indices.") // C3-based overloads: single-matrix overloads first. @@ -154,27 +243,64 @@ PYBIND11_MODULE(traj_eval, m) { "Compute cost from C3 with custom cost matrices.") // SimulatePDControlWithLCS overloads + // Binding: (vector, vector, VectorXd, VectorXd, + // LCS, bool, LCSSimulateConfig) .def_static( "SimulatePDControlWithLCS", static_cast, std::vector> (*)( const std::vector&, const std::vector&, const Eigen::VectorXd&, - const Eigen::VectorXd&, const LCS&)>( + const Eigen::VectorXd&, const LCS&, const bool&, + const c3::LCSSimulateConfig&)>( &traj_eval::TrajectoryEvaluator::SimulatePDControlWithLCS), py::arg("x_plan"), py::arg("u_plan"), py::arg("Kp"), py::arg("Kd"), - py::arg("lcs"), "Simulate PD control with an LCS") + py::arg("lcs"), py::arg("use_feedforward") = true, + py::arg("config") = c3::LCSSimulateConfig(), + "Simulate PD control with an LCS") + // Binding: (vector, VectorXd, VectorXd, LCS, + // LCSSimulateConfig) + .def_static( + "SimulatePDControlWithLCS", + static_cast, + std::vector> (*)( + const std::vector&, const Eigen::VectorXd&, + const Eigen::VectorXd&, const LCS&, + const c3::LCSSimulateConfig&)>( + &traj_eval::TrajectoryEvaluator::SimulatePDControlWithLCS), + py::arg("x_plan"), py::arg("Kp"), py::arg("Kd"), py::arg("lcs"), + py::arg("config") = c3::LCSSimulateConfig(), + "Simulate PD control with an LCS without feedforward") + // Binding: (vector, vector, VectorXd, VectorXd, + // LCS, LCS, bool, LCSSimulateConfig) .def_static( "SimulatePDControlWithLCS", static_cast, std::vector> (*)( const std::vector&, const std::vector&, const Eigen::VectorXd&, - const Eigen::VectorXd&, const LCS&, const LCS&)>( + const Eigen::VectorXd&, const LCS&, const LCS&, const bool&, + const c3::LCSSimulateConfig&)>( &traj_eval::TrajectoryEvaluator::SimulatePDControlWithLCS), py::arg("x_plan"), py::arg("u_plan"), py::arg("Kp"), py::arg("Kd"), py::arg("coarse_lcs"), py::arg("fine_lcs"), + py::arg("use_feedforward") = true, + py::arg("config") = c3::LCSSimulateConfig(), "Simulate PD control with coarse and fine LCSs") + // Binding: (vector, VectorXd, VectorXd, LCS, LCS, + // LCSSimulateConfig) + .def_static( + "SimulatePDControlWithLCS", + static_cast, + std::vector> (*)( + const std::vector&, const Eigen::VectorXd&, + const Eigen::VectorXd&, const LCS&, const LCS&, + const c3::LCSSimulateConfig&)>( + &traj_eval::TrajectoryEvaluator::SimulatePDControlWithLCS), + py::arg("x_plan"), py::arg("Kp"), py::arg("Kd"), + py::arg("coarse_lcs"), py::arg("fine_lcs"), + py::arg("config") = c3::LCSSimulateConfig(), + "Simulate PD control with coarse and fine LCSs without feedforward") // SimulateLCSOverTrajectory overloads .def_static( diff --git a/core/test/core_test.cc b/core/test/core_test.cc index 1aa98524..72611237 100644 --- a/core/test/core_test.cc +++ b/core/test/core_test.cc @@ -79,7 +79,7 @@ TEST_F(C3CartpoleTest, InitializationTest) { // Test if GetLinearConstraints is working as expected TEST_F(C3CartpoleTest, LinearConstraintsTest) { - std::vector user_constraints; + vector user_constraints; ASSERT_NO_THROW({ user_constraints = pOpt->GetLinearConstraints(); }); ASSERT_EQ(user_constraints.size(), 0); } @@ -117,7 +117,7 @@ TEST_P(C3CartpoleTestParameterizedLinearConstraints, LinearConstraintsTest) { pOpt->AddLinearConstraint(Al, lb, ub, constraint_type); } - std::vector user_constraints = + vector user_constraints = pOpt->GetLinearConstraints(); // Number of constraints must be N-1 for state and N for input and lambda EXPECT_EQ(user_constraints.size(), num_of_new_constraints); @@ -152,7 +152,7 @@ TEST_F(C3CartpoleTest, RemoveLinearConstraintsTest) { pOpt->AddLinearConstraint(Al, lb, ub, c3::ConstraintVariable::STATE); - std::vector user_constraints = + vector user_constraints = pOpt->GetLinearConstraints(); EXPECT_EQ(user_constraints.size(), N - 1); @@ -177,8 +177,7 @@ TEST_F(C3CartpoleTest, UpdateTargetTest) { pOpt->UpdateTarget(xdesired); - std::vector target_costs = - pOpt->GetTargetCost(); + vector target_costs = pOpt->GetTargetCost(); EXPECT_EQ(target_costs.size(), N + 1); for (int i = 0; i < N + 1; ++i) { @@ -214,8 +213,7 @@ TEST_F(C3CartpoleTest, UpdateCostMatrix) { } // Ensure target state costs are updated - std::vector target_costs = - pOpt->GetTargetCost(); + vector target_costs = pOpt->GetTargetCost(); for (int i = 0; i < N + 1; ++i) { // Quadratic Q and b cost matrices should be updated @@ -251,7 +249,7 @@ TEST_P(C3CartpoleTestParameterizedScalingLCSTest, ScalingLCSTest) { LinEq.block(0, n, n, m) = pSystem->D().at(0); LinEq.block(0, n + m, n, k) = pSystem->B().at(0); - std::vector dynamic_constraints = + vector dynamic_constraints = optimizer.GetDynamicConstraints(); for (int i = 0; i < N; ++i) { // Linear Equality A matrix should be updated @@ -283,9 +281,9 @@ TEST_F(C3CartpoleTest, ZSolStaleTest) { int timesteps = 5; // number of timesteps for the simulation // Create state and input arrays - std::vector z(timesteps, VectorXd::Zero(n + m + k)); - std::vector state(timesteps, VectorXd::Zero(n)); - std::vector input(timesteps, VectorXd::Zero(k)); + vector z(timesteps, VectorXd::Zero(n + m + k)); + vector state(timesteps, VectorXd::Zero(n)); + vector input(timesteps, VectorXd::Zero(k)); state[0] << 0.1, -0.5, 0.5, -0.4; // initial state with contact to right wall @@ -327,12 +325,12 @@ TEST_F(C3CartpoleTest, WarmStartSmokeTest) { class TrajectoryEvaluatorTest : public testing::Test {}; TEST_F(TrajectoryEvaluatorTest, QuadraticCostMatchesManual) { - std::vector x(2, VectorXd::Zero(1)); - std::vector u(1, VectorXd::Zero(1)); - std::vector lambda(1, VectorXd::Zero(1)); - std::vector x_des(2, VectorXd::Zero(1)); - std::vector u_des(1, VectorXd::Zero(1)); - std::vector lambda_des(1, VectorXd::Zero(1)); + vector x(2, VectorXd::Zero(1)); + vector u(1, VectorXd::Zero(1)); + vector lambda(1, VectorXd::Zero(1)); + vector x_des(2, VectorXd::Zero(1)); + vector u_des(1, VectorXd::Zero(1)); + vector lambda_des(1, VectorXd::Zero(1)); x[0](0) = 1.0; x[1](0) = 2.0; @@ -343,9 +341,9 @@ TEST_F(TrajectoryEvaluatorTest, QuadraticCostMatchesManual) { u_des[0](0) = 2.0; lambda_des[0](0) = 0.0; - std::vector Q(2, MatrixXd::Zero(1, 1)); - std::vector R(1, MatrixXd::Zero(1, 1)); - std::vector S(1, MatrixXd::Zero(1, 1)); + vector Q(2, MatrixXd::Zero(1, 1)); + vector R(1, MatrixXd::Zero(1, 1)); + vector S(1, MatrixXd::Zero(1, 1)); Q[0](0, 0) = 2.0; Q[1](0, 0) = 4.0; R[0](0, 0) = 3.0; @@ -402,31 +400,96 @@ TEST_F(TrajectoryEvaluatorTest, QuadraticCostMatchesManual) { 1e-12); // State costs with no desired trajectory // Test any mismatched dimensions throw errors. - std::vector x_wrong(3, VectorXd::Zero(1)); + vector x_wrong(3, VectorXd::Zero(1)); ASSERT_ANY_THROW( TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(x_wrong, x_des, Q)); - std::vector u_wrong(2, VectorXd::Zero(1)); + vector u_wrong(2, VectorXd::Zero(1)); ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( x, x_des, Q, u_wrong, u_des, R)); - std::vector lambda_wrong(2, VectorXd::Zero(1)); + vector lambda_wrong(2, VectorXd::Zero(1)); ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( x, x_des, Q, u, u_des, R, lambda_wrong, lambda_des, S)); - std::vector Q_wrong_size(2, MatrixXd::Zero(2, 2)); + vector Q_wrong_size(2, MatrixXd::Zero(2, 2)); ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( x, x_des, Q_wrong_size)); - std::vector R_wrong_size(1, MatrixXd::Zero(2, 2)); + vector R_wrong_size(1, MatrixXd::Zero(2, 2)); ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( x, x_des, Q, u, u_des, R_wrong_size)); - std::vector S_wrong_size(1, MatrixXd::Zero(2, 2)); + vector S_wrong_size(1, MatrixXd::Zero(2, 2)); ASSERT_ANY_THROW(TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( x, x_des, Q, u, u_des, R, lambda, lambda_des, S_wrong_size)); } +TEST_F(TrajectoryEvaluatorTest, QuadraticCostWithStartEndIndexArguments) { + vector x(2, VectorXd::Zero(3)); + vector x_des(2, VectorXd::Zero(3)); + + x[0] << 1.0, 2.0, 3.0; + x[1] << 4.0, 5.0, 6.0; + x_des[0] << 0.5, 1.5, 1.0; + x_des[1] << 3.0, 4.5, 7.0; + + vector Q(2, MatrixXd::Zero(3, 3)); + Q[0](0, 0) = 1.0; + Q[0](1, 1) = 2.0; + Q[0](2, 2) = 3.0; + Q[1](0, 0) = 4.0; + Q[1](1, 1) = 5.0; + Q[1](2, 2) = 6.0; + + // Compute only over indices [1, 3). + const int start = 1; + const int end = 3; + double expected_subset = 0.0; + expected_subset += 2.0 * (2.0 - 1.5) * (2.0 - 1.5); + expected_subset += 3.0 * (3.0 - 1.0) * (3.0 - 1.0); + expected_subset += 5.0 * (5.0 - 4.5) * (5.0 - 4.5); + expected_subset += 6.0 * (6.0 - 7.0) * (6.0 - 7.0); + + double actual_subset = TrajectoryEvaluator::ComputeQuadraticTrajectoryCost( + start, end, x, x_des, Q); + EXPECT_NEAR(actual_subset, expected_subset, 1e-12); + + // Same subset computation, but with no desired trajectory (assumed zero). + double expected_subset_zero_des = 0.0; + expected_subset_zero_des += 2.0 * 2.0 * 2.0; + expected_subset_zero_des += 3.0 * 3.0 * 3.0; + expected_subset_zero_des += 5.0 * 5.0 * 5.0; + expected_subset_zero_des += 6.0 * 6.0 * 6.0; + + double actual_subset_zero_des = + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(start, end, x, Q); + EXPECT_NEAR(actual_subset_zero_des, expected_subset_zero_des, 1e-12); + + // Repeated-cost-matrix overload with start/end. + MatrixXd Q_repeat = MatrixXd::Zero(3, 3); + Q_repeat(0, 0) = 2.0; + Q_repeat(1, 1) = 3.0; + Q_repeat(2, 2) = 4.0; + double expected_repeat_subset = 0.0; + expected_repeat_subset += 3.0 * (2.0 - 1.5) * (2.0 - 1.5); + expected_repeat_subset += 4.0 * (3.0 - 1.0) * (3.0 - 1.0); + expected_repeat_subset += 3.0 * (5.0 - 4.5) * (5.0 - 4.5); + expected_repeat_subset += 4.0 * (6.0 - 7.0) * (6.0 - 7.0); + double actual_repeat_subset = + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(start, end, x, x_des, + Q_repeat); + EXPECT_NEAR(actual_repeat_subset, expected_repeat_subset, 1e-12); + + // Invalid ranges should throw. + ASSERT_ANY_THROW( + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(-1, 2, x, x_des, Q)); + ASSERT_ANY_THROW( + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(2, 1, x, x_des, Q)); + ASSERT_ANY_THROW( + TrajectoryEvaluator::ComputeQuadraticTrajectoryCost(0, 4, x, x_des, Q)); +} + TEST_F(TrajectoryEvaluatorTest, SimulatePDControlWithLCSTest) { const int n = 4; // 4 states: 2 positions, 2 velocities const int k = 2; // 2 inputs @@ -449,8 +512,8 @@ TEST_F(TrajectoryEvaluatorTest, SimulatePDControlWithLCSTest) { LCS lcs(A, B, D, d, E, F, H, c, N, dt); // Create a planned trajectory - std::vector x_plan(N + 1, VectorXd::Zero(n)); - std::vector u_plan(N, VectorXd::Zero(k)); + vector x_plan(N + 1, VectorXd::Zero(n)); + vector u_plan(N, VectorXd::Zero(k)); x_plan[0] << 0.0, 0.0, 0.0, 0.0; x_plan[1] << 0.5, 0.5, 0.0, 0.0; @@ -470,30 +533,65 @@ TEST_F(TrajectoryEvaluatorTest, SimulatePDControlWithLCSTest) { Kd(3) = 1.0; // Simulate with PD control - std::pair, std::vector> result = + std::pair, vector> result = TrajectoryEvaluator::SimulatePDControlWithLCS(x_plan, u_plan, Kp, Kd, lcs); - std::vector x_sim = result.first; - std::vector u_sim = result.second; + vector x_sim = result.first; + vector u_sim = result.second; EXPECT_EQ(x_sim.size(), x_plan.size()); EXPECT_EQ(u_sim.size(), u_plan.size()); EXPECT_TRUE(x_sim[0].isApprox(x_plan[0])); + // Test new overload with no feedforward argument. This should match using an + // explicit zero feedforward trajectory with use_feedforward = false. + std::pair, vector> result_no_ff = + TrajectoryEvaluator::SimulatePDControlWithLCS(x_plan, Kp, Kd, lcs); + std::pair, vector> result_zero_ff = + TrajectoryEvaluator::SimulatePDControlWithLCS( + x_plan, vector(N, VectorXd::Zero(k)), Kp, Kd, lcs, false); + EXPECT_EQ(result_no_ff.first.size(), result_zero_ff.first.size()); + EXPECT_EQ(result_no_ff.second.size(), result_zero_ff.second.size()); + for (int i = 0; i < N + 1; ++i) { + EXPECT_TRUE(result_no_ff.first[i].isApprox(result_zero_ff.first[i])); + } + for (int i = 0; i < N; ++i) { + EXPECT_TRUE(result_no_ff.second[i].isApprox(result_zero_ff.second[i])); + } + // Test with coarse and fine LCS (fine has 2x time resolution) LCS fine_lcs(A, B / 2.0, D, d, E, F, H, c, N * 2, dt / 2); - std::pair, std::vector> result_fine = + std::pair, vector> result_fine = TrajectoryEvaluator::SimulatePDControlWithLCS(x_plan, u_plan, Kp, Kd, lcs, fine_lcs); - std::vector x_sim_fine = result_fine.first; - std::vector u_sim_fine = result_fine.second; + vector x_sim_fine = result_fine.first; + vector u_sim_fine = result_fine.second; EXPECT_EQ(x_sim_fine.size(), x_plan.size()); EXPECT_EQ(u_sim_fine.size(), u_plan.size()); EXPECT_TRUE(x_sim_fine[0].isApprox(x_plan[0])); + // Test new coarse/fine overload with no feedforward argument. + std::pair, vector> result_fine_no_ff = + TrajectoryEvaluator::SimulatePDControlWithLCS(x_plan, Kp, Kd, lcs, + fine_lcs); + std::pair, vector> result_fine_zero_ff = + TrajectoryEvaluator::SimulatePDControlWithLCS( + x_plan, vector(N, VectorXd::Zero(k)), Kp, Kd, lcs, fine_lcs, + false); + EXPECT_EQ(result_fine_no_ff.first.size(), result_fine_zero_ff.first.size()); + EXPECT_EQ(result_fine_no_ff.second.size(), result_fine_zero_ff.second.size()); + for (int i = 0; i < N + 1; ++i) { + EXPECT_TRUE( + result_fine_no_ff.first[i].isApprox(result_fine_zero_ff.first[i])); + } + for (int i = 0; i < N; ++i) { + EXPECT_TRUE( + result_fine_no_ff.second[i].isApprox(result_fine_zero_ff.second[i])); + } + // Test error checking for mismatched dimensions VectorXd Kp_wrong_size = VectorXd::Zero(n + 1); ASSERT_ANY_THROW(TrajectoryEvaluator::SimulatePDControlWithLCS( @@ -511,12 +609,12 @@ TEST_F(TrajectoryEvaluatorTest, SimulatePDControlWithLCSTest) { } TEST_F(TrajectoryEvaluatorTest, ZeroOrderHoldAndDownsampleRoundTrip) { - std::vector coarse(2, VectorXd::Zero(2)); + vector coarse(2, VectorXd::Zero(2)); coarse[0] << 1.0, 2.0; coarse[1] << 3.0, 4.0; const int upsample_rate = 3; - std::vector fine = + vector fine = TrajectoryEvaluator::ZeroOrderHoldTrajectory(coarse, upsample_rate); EXPECT_EQ(fine.size(), coarse.size() * upsample_rate); for (int i = 0; i < upsample_rate; ++i) { @@ -524,7 +622,7 @@ TEST_F(TrajectoryEvaluatorTest, ZeroOrderHoldAndDownsampleRoundTrip) { EXPECT_TRUE(fine[i + upsample_rate].isApprox(coarse[1])); } - std::vector downsampled = + vector downsampled = TrajectoryEvaluator::DownsampleTrajectory(fine, upsample_rate); EXPECT_EQ(downsampled.size(), coarse.size()); EXPECT_TRUE(downsampled[0].isApprox(coarse[0])); @@ -532,20 +630,20 @@ TEST_F(TrajectoryEvaluatorTest, ZeroOrderHoldAndDownsampleRoundTrip) { } TEST_F(TrajectoryEvaluatorTest, MultiZeroOrderHoldAndDownsampleRoundTrip) { - std::vector x_coarse(3, VectorXd::Zero(2)); + vector x_coarse(3, VectorXd::Zero(2)); x_coarse[0] << 1.0, 2.0; x_coarse[1] << 3.0, 4.0; x_coarse[2] << 5.0, 6.0; - std::vector u_coarse(2, VectorXd::Zero(1)); + vector u_coarse(2, VectorXd::Zero(1)); u_coarse[0] << 7.0; u_coarse[1] << 8.0; const int upsample_rate = 3; - std::pair, std::vector> fine = + std::pair, vector> fine = TrajectoryEvaluator::ZeroOrderHoldTrajectories(x_coarse, u_coarse, upsample_rate); - std::vector x_fine = fine.first; - std::vector u_fine = fine.second; + vector x_fine = fine.first; + vector u_fine = fine.second; EXPECT_EQ(x_fine.size(), (x_coarse.size() - 1) * upsample_rate + 1); EXPECT_EQ(u_fine.size(), u_coarse.size() * upsample_rate); for (int i = 0; i < upsample_rate; ++i) { @@ -556,11 +654,11 @@ TEST_F(TrajectoryEvaluatorTest, MultiZeroOrderHoldAndDownsampleRoundTrip) { } EXPECT_TRUE(x_fine.back().isApprox(x_coarse.back())); - std::pair, std::vector> downsampled = + std::pair, vector> downsampled = TrajectoryEvaluator::DownsampleTrajectories(x_fine, u_fine, upsample_rate); - std::vector x_downsampled = downsampled.first; - std::vector u_downsampled = downsampled.second; + vector x_downsampled = downsampled.first; + vector u_downsampled = downsampled.second; EXPECT_EQ(x_downsampled.size(), 3); EXPECT_EQ(x_downsampled.size(), x_coarse.size()); EXPECT_EQ(u_downsampled.size(), 2); @@ -572,16 +670,16 @@ TEST_F(TrajectoryEvaluatorTest, MultiZeroOrderHoldAndDownsampleRoundTrip) { EXPECT_TRUE(u_downsampled[1].isApprox(u_coarse[1])); // Test any mismatched dimensions throw errors. - std::vector x_coarse_wrong(2, VectorXd::Zero(3)); + vector x_coarse_wrong(2, VectorXd::Zero(3)); ASSERT_ANY_THROW(TrajectoryEvaluator::ZeroOrderHoldTrajectories( x_coarse_wrong, u_coarse, upsample_rate)); - std::vector u_coarse_wrong(3, VectorXd::Zero(2)); + vector u_coarse_wrong(3, VectorXd::Zero(2)); ASSERT_ANY_THROW(TrajectoryEvaluator::ZeroOrderHoldTrajectories( x_coarse, u_coarse_wrong, upsample_rate)); - std::vector x_fine_wrong(2, VectorXd::Zero(3)); + vector x_fine_wrong(2, VectorXd::Zero(3)); ASSERT_ANY_THROW(TrajectoryEvaluator::DownsampleTrajectories( x_fine_wrong, u_fine, upsample_rate)); - std::vector u_fine_wrong(3, VectorXd::Zero(2)); + vector u_fine_wrong(3, VectorXd::Zero(2)); ASSERT_ANY_THROW(TrajectoryEvaluator::DownsampleTrajectories( x_fine, u_fine_wrong, upsample_rate)); } @@ -607,11 +705,11 @@ TEST_F(TrajectoryEvaluatorTest, VectorXd x_init = VectorXd::Zero(n); x_init << 1.0, 1.0; - std::vector u_plan(N, VectorXd::Zero(k)); + vector u_plan(N, VectorXd::Zero(k)); u_plan[0] << 1.0, 0.0; u_plan[1] << 0.0, 2.0; - std::vector x_sim = + vector x_sim = TrajectoryEvaluator::SimulateLCSOverTrajectory(x_init, u_plan, lcs); ASSERT_EQ(x_sim.size(), static_cast(N + 1)); @@ -623,7 +721,7 @@ TEST_F(TrajectoryEvaluatorTest, // Test with a finer LCS (smaller dt) LCS finer_lcs(A, B / 10.0, D, d, E, F, H, c, N * 10, dt / 10); - std::vector x_sim_from_finer = + vector x_sim_from_finer = TrajectoryEvaluator::SimulateLCSOverTrajectory(x_init, u_plan, lcs, finer_lcs); ASSERT_EQ(x_sim_from_finer.size(), static_cast(N + 1)); @@ -632,7 +730,7 @@ TEST_F(TrajectoryEvaluatorTest, EXPECT_TRUE(x_sim_from_finer[2].isApprox(x2_expected)); // Test any mismatched dimensions throw errors. - std::vector u_plan_wrong(2, VectorXd::Zero(3)); + vector u_plan_wrong(2, VectorXd::Zero(3)); ASSERT_ANY_THROW(TrajectoryEvaluator::SimulateLCSOverTrajectory( x_init, u_plan_wrong, lcs)); VectorXd x_init_wrong = VectorXd::Zero(3); @@ -658,8 +756,8 @@ TYPED_TEST_SUITE(C3CartpoleTypedTest, projection_types); TYPED_TEST(C3CartpoleTypedTest, UpdateLCSTest) { c3::C3* pOpt = this->pOpt.get(); auto dt = this->dt; - std::vector - pre_dynamic_constraints = pOpt->GetDynamicConstraints(); + vector pre_dynamic_constraints = + pOpt->GetDynamicConstraints(); auto& N = this->N; auto n = this->n; auto k = this->k; @@ -685,8 +783,8 @@ TYPED_TEST(C3CartpoleTypedTest, UpdateLCSTest) { pOpt->UpdateLCS(TestSystem); - std::vector - pst_dynamic_constraints = pOpt->GetDynamicConstraints(); + vector pst_dynamic_constraints = + pOpt->GetDynamicConstraints(); for (int i = 0; i < N; ++i) { // Linear Equality A matrix should be updated MatrixXd pst_Al = pst_dynamic_constraints[i]->GetDenseA(); @@ -700,8 +798,8 @@ TYPED_TEST(C3CartpoleTypedTest, End2EndCartpoleTest) { int timesteps = 1000; // number of timesteps for the simulation /// create state and input arrays - std::vector x(timesteps, VectorXd::Zero(this->n)); - std::vector input(timesteps, VectorXd::Zero(this->k)); + vector x(timesteps, VectorXd::Zero(this->n)); + vector input(timesteps, VectorXd::Zero(this->k)); x[0] = this->x0; @@ -732,9 +830,9 @@ TYPED_TEST(C3CartpoleTypedTest, ComputeCost) { // Solve one iteration of the problem. optimizer.Solve(this->x0); - std::vector x_sol = optimizer.GetStateSolution(); - std::vector u_sol = optimizer.GetInputSolution(); - std::vector lambda_sol = optimizer.GetForceSolution(); + vector x_sol = optimizer.GetStateSolution(); + vector u_sol = optimizer.GetInputSolution(); + vector lambda_sol = optimizer.GetForceSolution(); // The state trajectory excludes the N+1 time step. Add it to the end via // LCS rollout since it contributes to the C3 internal cost optimization. @@ -748,9 +846,9 @@ TYPED_TEST(C3CartpoleTypedTest, ComputeCost) { // Get the cost matrices and desired state. const C3::CostMatrices cost_matrices = optimizer.GetCostMatrices(); - std::vector Q = cost_matrices.Q; - std::vector R = cost_matrices.R; - std::vector x_des = optimizer.GetDesiredState(); + vector Q = cost_matrices.Q; + vector R = cost_matrices.R; + vector x_des = optimizer.GetDesiredState(); ASSERT_EQ(x_sol.size(), x_des.size()); ASSERT_EQ(x_sol.size(), Q.size()); ASSERT_EQ(u_sol.size(), R.size()); diff --git a/core/traj_eval.cc b/core/traj_eval.cc index c3847d5a..82674083 100644 --- a/core/traj_eval.cc +++ b/core/traj_eval.cc @@ -16,11 +16,10 @@ using Eigen::VectorXd; using std::vector; std::pair, vector> -TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, - const vector& u_plan, - const VectorXd& Kp, - const VectorXd& Kd, - const LCS& lcs) { +TrajectoryEvaluator::SimulatePDControlWithLCS( + const vector& x_plan, const vector& u_plan, + const VectorXd& Kp, const VectorXd& Kd, const LCS& lcs, + const bool& use_feedforward, const LCSSimulateConfig& config) { CheckLCSAndTrajectoryCompatibility(lcs, x_plan, u_plan); int N = lcs.N(); @@ -54,20 +53,34 @@ TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, x[0] = x_plan[0]; for (int i = 0; i < N; i++) { - u[i] = - u_plan[i] + Kp_mat * (x_plan[i] - x[i]) + Kd_mat * (x_plan[i] - x[i]); - x[i + 1] = lcs.Simulate(x[i], u[i]); + u[i] = Kp_mat * (x_plan[i] - x[i]) + Kd_mat * (x_plan[i] - x[i]); + if (use_feedforward) { + u[i] += u_plan[i]; + } + x[i + 1] = lcs.Simulate(x[i], u[i], config); } return std::make_pair(x, u); } std::pair, vector> TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, - const vector& u_plan, const VectorXd& Kp, const VectorXd& Kd, - const LCS& coarse_lcs, - const LCS& fine_lcs) { + const LCS& lcs, + const LCSSimulateConfig& config) { + const vector empty_u_plan(lcs.N(), + VectorXd::Zero(lcs.num_inputs())); + const bool use_feedforward = false; + return SimulatePDControlWithLCS(x_plan, empty_u_plan, Kp, Kd, lcs, + use_feedforward, config); +} + +std::pair, vector> +TrajectoryEvaluator::SimulatePDControlWithLCS( + const vector& x_plan, const vector& u_plan, + const VectorXd& Kp, const VectorXd& Kd, const LCS& coarse_lcs, + const LCS& fine_lcs, const bool& use_feedforward, + const LCSSimulateConfig& config) { int upsample_rate = CheckCoarseAndFineLCSCompatibility(coarse_lcs, fine_lcs); // Zero-order hold the planned trajectory to match the finer time @@ -76,8 +89,8 @@ TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, ZeroOrderHoldTrajectories(x_plan, u_plan, upsample_rate); // Do PD control with the finer trajectory and LCS. - auto [x_sim_fine, u_sim_fine] = - SimulatePDControlWithLCS(x_plan_finer, u_plan_finer, Kp, Kd, fine_lcs); + auto [x_sim_fine, u_sim_fine] = SimulatePDControlWithLCS( + x_plan_finer, u_plan_finer, Kp, Kd, fine_lcs, use_feedforward, config); // Downsample the resulting trajectory back to the original time // discretization. @@ -87,6 +100,20 @@ TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, return std::make_pair(x_sim, u_sim); } +std::pair, vector> +TrajectoryEvaluator::SimulatePDControlWithLCS(const vector& x_plan, + const VectorXd& Kp, + const VectorXd& Kd, + const LCS& coarse_lcs, + const LCS& fine_lcs, + const LCSSimulateConfig& config) { + const vector empty_u_plan(coarse_lcs.N(), + VectorXd::Zero(coarse_lcs.num_inputs())); + const bool use_feedforward = false; + return SimulatePDControlWithLCS(x_plan, empty_u_plan, Kp, Kd, coarse_lcs, + fine_lcs, use_feedforward, config); +} + vector TrajectoryEvaluator::SimulateLCSOverTrajectory( const VectorXd& x_init, const vector& u_plan, const LCS& lcs, const LCSSimulateConfig& config) { @@ -197,9 +224,9 @@ int TrajectoryEvaluator::CheckCoarseAndFineLCSCompatibility( } void TrajectoryEvaluator::CheckLCSAndTrajectoryCompatibility( - const LCS& lcs, const std::vector& x, - const std::optional>& u, - const std::optional>& lambda) { + const LCS& lcs, const vector& x, + const std::optional>& u, + const std::optional>& lambda) { DRAKE_THROW_UNLESS(lcs.N() == x.size() - 1); if (u.has_value()) { DRAKE_THROW_UNLESS(lcs.N() == u->size()); diff --git a/core/traj_eval.h b/core/traj_eval.h index 3a63441b..363cd392 100644 --- a/core/traj_eval.h +++ b/core/traj_eval.h @@ -38,8 +38,14 @@ class TrajectoryEvaluator { * each individual cost is computed as: * * Cost = Sum_{i = 0, ... N-1} - * (data_i-data_des_i)^T CostMatrix_i (data_i-data_des_i) + * (data_i[start_index:end_index]-data_des_i[start_index:end_index])^T + * CostMatrix_i[start_index:end_index, start_index:end_index] + * (data_i[start_index:end_index]-data_des_i[start_index:end_index]) * + * @param start_index Starting index of the data vectors over which to compute + * costs (inclusive) + * @param end_index Ending index of the data vectors over which to compute + * costs (exclusive) * @param data Trajectory of length N of vectors * @param data_des Trajectory of length N of desired vectors * @param cost_matrices Trajectory of length N of cost matrices. This can be @@ -60,12 +66,19 @@ class TrajectoryEvaluator { typename = std::enable_if_t::value && detail::IsCostType::value>> static double ComputeQuadraticTrajectoryCost( + const int& start_index, const int& end_index, const std::vector& data, const T_des& data_des, const T_cost& cost_matrices, Rest&&... rest) { double cost = 0.0; int N = data.size(); + DRAKE_THROW_UNLESS(N > 0); int n_data = data[0].size(); + DRAKE_THROW_UNLESS(start_index >= 0); + DRAKE_THROW_UNLESS(end_index >= start_index); + DRAKE_THROW_UNLESS(end_index <= n_data); + int subset_size = end_index - start_index; + // Handle if data_des is provided as a single vector instead of a trajectory // of vectors. std::vector data_des_vec; @@ -97,12 +110,32 @@ class TrajectoryEvaluator { DRAKE_THROW_UNLESS(cost_matrices_vec[i].rows() == n_data); DRAKE_THROW_UNLESS(cost_matrices_vec[i].cols() == n_data); - cost += (data[i] - data_des_vec[i]).transpose() * cost_matrices_vec[i] * - (data[i] - data_des_vec[i]); + cost += (data[i] - data_des_vec[i]) + .segment(start_index, subset_size) + .transpose() * + cost_matrices_vec[i].block(start_index, start_index, subset_size, + subset_size) * + (data[i] - data_des_vec[i]).segment(start_index, subset_size); } return cost + ComputeQuadraticTrajectoryCost(std::forward(rest)...); } + + /** @brief Same as above, but no start_index and end_index are provided, so + * the cost is computed over the entire data vectors. + */ + template ::value && + detail::IsCostType::value>> + static double ComputeQuadraticTrajectoryCost( + const std::vector& data, const T_des& data_des, + const T_cost& cost_matrices, Rest&&... rest) { + DRAKE_THROW_UNLESS(!data.empty()); + return ComputeQuadraticTrajectoryCost(0, data[0].size(), data, data_des, + cost_matrices, + std::forward(rest)...); + } + /** @brief Special case: no desired data is provided, so it is assumed the * desired data is zero. */ @@ -111,12 +144,30 @@ class TrajectoryEvaluator { static double ComputeQuadraticTrajectoryCost( const std::vector& data, const T_cost& cost_matrices, Rest&&... rest) { + DRAKE_THROW_UNLESS(!data.empty()); return ComputeQuadraticTrajectoryCost( data, std::vector(data.size(), Eigen::VectorXd::Zero(data[0].size())), cost_matrices, std::forward(rest)...); } + + /** @brief Same as above but computes cost only over [start_index, + * end_index). + */ + template ::value>> + static double ComputeQuadraticTrajectoryCost( + const int& start_index, const int& end_index, + const std::vector& data, const T_cost& cost_matrices, + Rest&&... rest) { + DRAKE_THROW_UNLESS(!data.empty()); + return ComputeQuadraticTrajectoryCost( + start_index, end_index, data, + std::vector(data.size(), + Eigen::VectorXd::Zero(data[0].size())), + cost_matrices, std::forward(rest)...); + } /** @brief Special case: a C3 object is the input argument, and the cost is to * be computed based on the trajectories contained in the C3 object and the * cost matrices contained in the C3 object for state and input costs. @@ -205,13 +256,26 @@ class TrajectoryEvaluator { * @param Kd Derivative gains (length n_x, with exactly k non-zero * entries) * @param lcs LCS system to simulate + * @param use_feedforward Whether to include feedforward control from the plan + * @param config Configuration for simulating the LCS * @return Pair of (simulated states, simulated inputs) */ static std::pair, std::vector> - SimulatePDControlWithLCS(const std::vector& x_plan, - const std::vector& u_plan, - const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd, - const LCS& lcs); + SimulatePDControlWithLCS( + const std::vector& x_plan, + const std::vector& u_plan, const Eigen::VectorXd& Kp, + const Eigen::VectorXd& Kd, const LCS& lcs, + const bool& use_feedforward = true, + const LCSSimulateConfig& config = LCSSimulateConfig()); + /** + * @brief Special case: the input plan is not provided, so use no feedforward + * control from the plan. + */ + static std::pair, std::vector> + SimulatePDControlWithLCS( + const std::vector& x_plan, const Eigen::VectorXd& Kp, + const Eigen::VectorXd& Kd, const LCS& lcs, + const LCSSimulateConfig& config = LCSSimulateConfig()); /** * @brief Special case: simulate plans from a coarser LCS with a finer LCS. * The returned trajectory is downsampled back to be compatible with the @@ -219,10 +283,21 @@ class TrajectoryEvaluator { * and input plans must be compatible with the coarser LCS. */ static std::pair, std::vector> - SimulatePDControlWithLCS(const std::vector& x_plan, - const std::vector& u_plan, - const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd, - const LCS& coarse_lcs, const LCS& fine_lcs); + SimulatePDControlWithLCS( + const std::vector& x_plan, + const std::vector& u_plan, const Eigen::VectorXd& Kp, + const Eigen::VectorXd& Kd, const LCS& coarse_lcs, const LCS& fine_lcs, + const bool& use_feedforward = true, + const LCSSimulateConfig& config = LCSSimulateConfig()); + /** + * *@brief Special case: simulate plans from a coarser LCS with a finer LCS, + * and the input plan is not provided. + */ + static std::pair, std::vector> + SimulatePDControlWithLCS( + const std::vector& x_plan, const Eigen::VectorXd& Kp, + const Eigen::VectorXd& Kd, const LCS& coarse_lcs, const LCS& fine_lcs, + const LCSSimulateConfig& config = LCSSimulateConfig()); /** * @brief Simulate an LCS forward from an initial condition over a trajectory