feat(c3) : Add ability to evaluate cost of C3 plan

bindings/pyc3/BUILD.bazel

@@ -24,6 +24,19 @@ pybind_py_library(
     py_imports = ["."],
 )
 
+pybind_py_library(
+    name = "traj_eval_py",
+    cc_deps = ["//core:lcs",
+               "//core:traj_eval",
+               "//core:c3"],
+    cc_so_name = "traj_eval",
+    cc_srcs = ["traj_eval_py.cc"],
+    py_deps = [
+        ":module_py",
+    ],
+    py_imports = ["."],
+)
+
 pybind_py_library(
     name = "c3_multibody_py",
     cc_deps = ["//multibody:lcs_factory",
@@ -75,6 +88,7 @@ py_library(
 PY_LIBRARIES = [
     ":module_py",
     ":c3_py",
+    ":traj_eval_py",
     ":c3_systems_py",
     ":c3_multibody_py",
 ]

bindings/pyc3/__init__.py

@@ -2,5 +2,6 @@
 import pydrake
 from . import *
 from .c3 import *
+from .traj_eval import *
 from .systems import *
 from .multibody import *

bindings/pyc3/traj_eval_py.cc

@@ -0,0 +1,223 @@
+#include <Eigen/Dense>
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include "core/lcs.h"
+#include "core/traj_eval.h"
+
+namespace py = pybind11;
+
+namespace c3 {
+namespace pyc3 {
+
+PYBIND11_MODULE(traj_eval, m) {
+  // NOTE: C++ API Flexibility vs. Python Bindings Limitation
+  // The C++ TrajectoryEvaluator::ComputeQuadraticTrajectoryCost() uses variadic
+  // templates to support arbitrary input combinations (e.g.,
+  // ComputeQuadraticTrajectoryCost(x, x_des, Q, u, R)). However, Python's
+  // pybind11 does not support variadic template forwarding natively. Therefore,
+  // the Python bindings only expose a finite set of predefined overloads. If
+  // additional input combinations are needed for Python use cases, they must be
+  // explicitly added to this file with corresponding static_cast declarations.
+
+  // LCSSimulateConfig struct bindings
+  py::class_<LCSSimulateConfig>(m, "LCSSimulateConfig")
+      .def(py::init<>())
+      .def_readwrite("regularized", &LCSSimulateConfig::regularized)
+      .def_readwrite("piv_tol", &LCSSimulateConfig::piv_tol)
+      .def_readwrite("zero_tol", &LCSSimulateConfig::zero_tol)
+      .def_readwrite("min_exp", &LCSSimulateConfig::min_exp)
+      .def_readwrite("step_exp", &LCSSimulateConfig::step_exp)
+      .def_readwrite("max_exp", &LCSSimulateConfig::max_exp);
+
+  // TrajectoryEvaluator class bindings
+  py::class_<traj_eval::TrajectoryEvaluator>(m, "TrajectoryEvaluator")
+
+      // ComputeQuadraticTrajectoryCost overloads
+      .def_static(
+          "ComputeQuadraticTrajectoryCost",
+          static_cast<double (*)(const std::vector<Eigen::VectorXd>&,
+                                 const std::vector<Eigen::VectorXd>&,
+                                 const std::vector<Eigen::MatrixXd>&)>(
+              &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost),
+          py::arg("data"), py::arg("data_des"), py::arg("cost_matrices"),
+          "Compute quadratic cost for full trajectory cost computation")
+      .def_static(
+          "ComputeQuadraticTrajectoryCost",
+          static_cast<double (*)(const std::vector<Eigen::VectorXd>&,
+                                 const std::vector<Eigen::VectorXd>&,
+                                 const Eigen::MatrixXd&)>(
+              &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost),
+          py::arg("data"), py::arg("data_des"), py::arg("cost_matrix"),
+          "Compute quadratic cost with single cost matrix")
+      .def_static(
+          "ComputeQuadraticTrajectoryCost",
+          static_cast<double (*)(const std::vector<Eigen::VectorXd>&,
+                                 const Eigen::VectorXd&,
+                                 const std::vector<Eigen::MatrixXd>&)>(
+              &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost),
+          py::arg("data"), py::arg("data_des"), py::arg("cost_matrices"),
+          "Compute quadratic cost with single desired vector")
+      .def_static(
+          "ComputeQuadraticTrajectoryCost",
+          static_cast<double (*)(const std::vector<Eigen::VectorXd>&,
+                                 const Eigen::VectorXd&,
+                                 const Eigen::MatrixXd&)>(
+              &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost),
+          py::arg("data"), py::arg("data_des"), py::arg("cost_matrix"),
+          "Compute quadratic cost with single desired vector and single cost "
+          "matrix")
+      .def_static(
+          "ComputeQuadraticTrajectoryCost",
+          static_cast<double (*)(const std::vector<Eigen::VectorXd>&,
+                                 const std::vector<Eigen::MatrixXd>&)>(
+              &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost),
+          py::arg("data"), py::arg("cost_matrices"),
+          "Compute quadratic cost assuming zero desired data")
+      .def_static(
+          "ComputeQuadraticTrajectoryCost",
+          static_cast<double (*)(const std::vector<Eigen::VectorXd>&,
+                                 const Eigen::MatrixXd&)>(
+              &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost),
+          py::arg("data"), py::arg("cost_matrix"),
+          "Compute quadratic cost with single matrix and zero desired")
+      .def_static(
+          "ComputeQuadraticTrajectoryCost",
+          static_cast<double (*)(C3*)>(
+              &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost),
+          py::arg("c3"), "Compute cost from C3 optimizer solution")
+      .def_static(
+          "ComputeQuadraticTrajectoryCost",
+          static_cast<double (*)(C3*, const std::vector<Eigen::MatrixXd>&,
+                                 const std::vector<Eigen::MatrixXd>&)>(
+              &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost),
+          py::arg("c3"), py::arg("Q"), py::arg("R"),
+          "Compute cost from C3 with custom cost matrices")
+      .def_static(
+          "ComputeQuadraticTrajectoryCost",
+          static_cast<double (*)(C3*, const Eigen::MatrixXd&,
+                                 const std::vector<Eigen::MatrixXd>&)>(
+              &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost),
+          py::arg("c3"), py::arg("Q"), py::arg("R"),
+          "Compute cost from C3 with single Q matrix")
+      .def_static(
+          "ComputeQuadraticTrajectoryCost",
+          static_cast<double (*)(C3*, const std::vector<Eigen::MatrixXd>&,
+                                 const Eigen::MatrixXd&)>(
+              &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost),
+          py::arg("c3"), py::arg("Q"), py::arg("R"),
+          "Compute cost from C3 with single R matrix")
+      .def_static(
+          "ComputeQuadraticTrajectoryCost",
+          static_cast<double (*)(C3*, const Eigen::MatrixXd&,
+                                 const Eigen::MatrixXd&)>(
+              &traj_eval::TrajectoryEvaluator::ComputeQuadraticTrajectoryCost),
+          py::arg("c3"), py::arg("Q"), py::arg("R"),
+          "Compute cost from C3 with single Q and R matrices")
+
+      // SimulatePDControlWithLCS overloads
+      .def_static(
+          "SimulatePDControlWithLCS",
+          static_cast<std::pair<std::vector<Eigen::VectorXd>,
+                                std::vector<Eigen::VectorXd>> (*)(
+              const std::vector<Eigen::VectorXd>&,
+              const std::vector<Eigen::VectorXd>&, const Eigen::VectorXd&,
+              const Eigen::VectorXd&, const LCS&)>(
+              &traj_eval::TrajectoryEvaluator::SimulatePDControlWithLCS),
+          py::arg("x_plan"), py::arg("u_plan"), py::arg("Kp"), py::arg("Kd"),
+          py::arg("lcs"), "Simulate PD control with an LCS")
+      .def_static(
+          "SimulatePDControlWithLCS",
+          static_cast<std::pair<std::vector<Eigen::VectorXd>,
+                                std::vector<Eigen::VectorXd>> (*)(
+              const std::vector<Eigen::VectorXd>&,
+              const std::vector<Eigen::VectorXd>&, const Eigen::VectorXd&,
+              const Eigen::VectorXd&, const LCS&, const LCS&)>(
+              &traj_eval::TrajectoryEvaluator::SimulatePDControlWithLCS),
+          py::arg("x_plan"), py::arg("u_plan"), py::arg("Kp"), py::arg("Kd"),
+          py::arg("coarse_lcs"), py::arg("fine_lcs"),
+          "Simulate PD control with coarse and fine LCSs")
+
+      // SimulateLCSOverTrajectory overloads
+      .def_static(
+          "SimulateLCSOverTrajectory",
+          static_cast<std::vector<Eigen::VectorXd> (*)(
+              const Eigen::VectorXd&, const std::vector<Eigen::VectorXd>&,
+              const LCS&, const c3::LCSSimulateConfig&)>(
+              &traj_eval::TrajectoryEvaluator::SimulateLCSOverTrajectory),
+          py::arg("x_init"), py::arg("u_plan"), py::arg("lcs"),
+          py::arg("config") = c3::LCSSimulateConfig(),
+          "Simulate LCS over input trajectory from initial state")
+      .def_static(
+          "SimulateLCSOverTrajectory",
+          static_cast<std::vector<Eigen::VectorXd> (*)(
+              const std::vector<Eigen::VectorXd>&,
+              const std::vector<Eigen::VectorXd>&, const LCS&,
+              const c3::LCSSimulateConfig&)>(
+              &traj_eval::TrajectoryEvaluator::SimulateLCSOverTrajectory),
+          py::arg("x_plan"), py::arg("u_plan"), py::arg("lcs"),
+          py::arg("config") = c3::LCSSimulateConfig(),
+          "Simulate LCS over trajectory using initial state from plan")
+      .def_static(
+          "SimulateLCSOverTrajectory",
+          static_cast<std::vector<Eigen::VectorXd> (*)(
+              const Eigen::VectorXd&, const std::vector<Eigen::VectorXd>&,
+              const LCS&, const LCS&, const c3::LCSSimulateConfig&)>(
+              &traj_eval::TrajectoryEvaluator::SimulateLCSOverTrajectory),
+          py::arg("x_init"), py::arg("u_plan"), py::arg("coarse_lcs"),
+          py::arg("fine_lcs"), py::arg("config") = c3::LCSSimulateConfig(),
+          "Simulate with coarse and fine LCSs from initial state")
+      .def_static(
+          "SimulateLCSOverTrajectory",
+          static_cast<std::vector<Eigen::VectorXd> (*)(
+              const std::vector<Eigen::VectorXd>&,
+              const std::vector<Eigen::VectorXd>&, const LCS&, const LCS&,
+              const c3::LCSSimulateConfig&)>(
+              &traj_eval::TrajectoryEvaluator::SimulateLCSOverTrajectory),
+          py::arg("x_plan"), py::arg("u_plan"), py::arg("coarse_lcs"),
+          py::arg("fine_lcs"), py::arg("config") = c3::LCSSimulateConfig(),
+          "Simulate with coarse and fine LCSs using plan initial state")
+
+      // ZeroOrderHold methods
+      .def_static("ZeroOrderHoldTrajectories",
+                  &traj_eval::TrajectoryEvaluator::ZeroOrderHoldTrajectories,
+                  py::arg("x_coarse"), py::arg("u_coarse"),
+                  py::arg("upsample_rate"),
+                  "Upsample state and input trajectories")
+      .def_static("ZeroOrderHoldTrajectory",
+                  &traj_eval::TrajectoryEvaluator::ZeroOrderHoldTrajectory,
+                  py::arg("coarse_traj"), py::arg("upsample_rate"),
+                  "Upsample a single trajectory")
+
+      // Downsample methods
+      .def_static("DownsampleTrajectories",
+                  &traj_eval::TrajectoryEvaluator::DownsampleTrajectories,
+                  py::arg("x_fine"), py::arg("u_fine"),
+                  py::arg("downsample_rate"),
+                  "Downsample state and input trajectories")
+      .def_static("DownsampleTrajectory",
+                  &traj_eval::TrajectoryEvaluator::DownsampleTrajectory,
+                  py::arg("fine_traj"), py::arg("downsample_rate"),
+                  "Downsample a single trajectory")
+
+      // LCS compatibility checking methods
+      .def_static(
+          "CheckCoarseAndFineLCSCompatibility",
+          &traj_eval::TrajectoryEvaluator::CheckCoarseAndFineLCSCompatibility,
+          py::arg("coarse_lcs"), py::arg("fine_lcs"),
+          "Check LCS time discretization compatibility")
+      .def_static("CheckLCSAndTrajectoryCompatibility",
+                  static_cast<void (*)(
+                      const LCS&, const std::vector<Eigen::VectorXd>&,
+                      const std::optional<std::vector<Eigen::VectorXd>>&,
+                      const std::optional<std::vector<Eigen::VectorXd>>&)>(
+                      &traj_eval::TrajectoryEvaluator::
+                          CheckLCSAndTrajectoryCompatibility),
+                  py::arg("lcs"), py::arg("x"), py::arg("u") = py::none(),
+                  py::arg("lambda") = py::none(),
+                  "Check trajectory compatibility with LCS; inputs and lambdas "
+                  "are optional");
+}
+}  // namespace pyc3
+}  // namespace c3

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 = [