Narrow Phase Kernel and full scale testing of SYCL Proximity engine with Drake

.gitignore

@@ -7,6 +7,9 @@
 *~
 .#*
 *.bak
+**.pyc
+__pycache__/
+*.pyc
 # - Directories (at top level)
 /xcode/
 /.cproject/
@@ -29,3 +32,10 @@
 
 # User-specified configuration
 /user.bazelrc
+
+# profiling reports
+*.ncu-rep
+*.nsys-rep
+*.sqlite
+*.png
+*.csv

MODULE.bazel

@@ -31,6 +31,8 @@ cc_configure = use_extension(
 )
 use_repo(cc_configure, "local_config_cc")
 
+register_toolchains("//tools/dpcpp_toolchain:dpcpp_toolchain_for_linux")
+
 # Add C++ dependencies that are part of our public API.
 #
 # We declare these dependencies twice -- as modules (bazel_dep) and repository

bindings/pydrake/BUILD.bazel

@@ -57,7 +57,11 @@ generate_pybind_documentation_header(
     name = "generate_pybind_documentation_header",
     out = "documentation_pybind.h",
     # Anonymous namespace and deduction guides both confuse pybind.
-    exclude_hdr_patterns = ["drake/common/overloaded.h"],
+    # SYCL headers require Intel OneAPI headers that aren't available during doc generation.
+    exclude_hdr_patterns = [
+        "drake/common/overloaded.h",
+        "drake/geometry/proximity/sycl/**",
+    ],
     root_name = "pydrake_doc",
     targets = [
         "//tools/install/libdrake:drake_headers",

bindings/pydrake/multibody/plant_py.cc

@@ -275,7 +275,7 @@ void DoScalarDependentDefinitions(py::module m, T) {
             cls_doc.RenameModelInstance.doc)
         .def(
             "AddRigidBody",
-            [](Class* self, const std::string& name,
+            [](Class * self, const std::string& name,
                 const SpatialInertia<double>& s) -> auto& {
               return self->AddRigidBody(name, s);
             },
@@ -344,7 +344,13 @@ void DoScalarDependentDefinitions(py::module m, T) {
             py::arg("X_AP"), py::arg("body_B"), py::arg("X_BQ"),
             py_rvp::reference_internal, cls_doc.AddWeldConstraint.doc)
         .def("RemoveConstraint", &Class::RemoveConstraint, py::arg("id"),
-            cls_doc.RemoveConstraint.doc);
+            cls_doc.RemoveConstraint.doc)
+        .def("set_sycl_for_hydroelastic_contact",
+            &Class::set_sycl_for_hydroelastic_contact, py::arg("use_sycl"),
+            cls_doc.set_sycl_for_hydroelastic_contact.doc)
+        .def("is_sycl_for_hydroelastic_contact",
+            &Class::is_sycl_for_hydroelastic_contact,
+            cls_doc.is_sycl_for_hydroelastic_contact.doc);
     // Mathy bits
     cls  // BR
         .def(
@@ -947,7 +953,7 @@ void DoScalarDependentDefinitions(py::module m, T) {
             py_rvp::reference_internal, cls_doc.GetJointByName.doc)
         .def(
             "GetMutableJointByName",
-            [](Class* self, string_view name,
+            [](Class * self, string_view name,
                 std::optional<ModelInstanceIndex> model_instance) -> auto& {
               return self->GetMutableJointByName(name, model_instance);
             },
@@ -1352,7 +1358,10 @@ void DoScalarDependentDefinitions(py::module m, T) {
                 bool>(&Class::GetActuatorNames),
             py::arg("model_instance"),
             py::arg("add_model_instance_prefix") = false,
-            cls_doc.GetActuatorNames.doc_2args);
+            cls_doc.GetActuatorNames.doc_2args)
+        .def("PrintPerformanceStats", &Class::PrintPerformanceStats,
+            py::arg("scene_graph"), py::arg("context"),
+            py::arg("base_json_path"), py::arg("sim_time") = 0.0);
   }
 
   {

bindings/pydrake/multibody/tree_py.cc

@@ -935,6 +935,8 @@ void DoScalarDependentDefinitions(py::module m, T) {
         .def("input_start", &Class::input_start, cls_doc.input_start.doc)
         .def("num_inputs", &Class::num_inputs, cls_doc.num_inputs.doc)
         .def("effort_limit", &Class::effort_limit, cls_doc.effort_limit.doc)
+        .def("set_effort_limit", &Class::set_effort_limit,
+            py::arg("effort_limit"), cls_doc.set_effort_limit.doc)
         .def("default_rotor_inertia", &Class::default_rotor_inertia,
             cls_doc.default_rotor_inertia.doc)
         .def("default_gear_ratio", &Class::default_gear_ratio,

common/BUILD.bazel

@@ -597,12 +597,18 @@ drake_cc_library(
 drake_cc_library(
     name = "timer",
     srcs = ["timer.cc"],
-    hdrs = ["timer.h"],
+    hdrs = ["timer.h", "cpu_timing_logger.h"],
     deps = [
         ":essential",
     ],
 )
 
+drake_cc_library(
+    name = "problem_size_logger",
+    hdrs = ["problem_size_logger.h"],
+    deps = [":essential"],
+)
+
 drake_cc_library(
     name = "value",
     srcs = ["value.cc"],

common/cpu_timing_logger.h

@@ -0,0 +1,186 @@
+#pragma once
+
+#include <algorithm>
+#include <chrono>
+#include <fstream>
+#include <iostream>
+#include <map>
+#include <mutex>
+#include <numeric>
+#include <set>
+#include <sstream>
+#include <string>
+#include <vector>
+
+namespace drake {
+namespace common {
+
+// Define this macro to enable CPU timing logging
+#define DRAKE_CPU_TIMING_ENABLED
+
+#ifdef DRAKE_CPU_TIMING_ENABLED
+
+class CpuTimingLogger {
+ public:
+  struct TimingStats {
+    double min_time_us = 0.0;
+    double max_time_us = 0.0;
+    double avg_time_us = 0.0;
+    double total_time_us = 0.0;
+    size_t call_count = 0;
+  };
+
+  static CpuTimingLogger& GetInstance() {
+    static CpuTimingLogger instance;
+    return instance;
+  }
+
+  void AddTiming(const std::string& name, double microseconds) {
+    if (!enabled_) return;
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (first_run_seen_.find(name) == first_run_seen_.end()) {
+      first_run_seen_.insert(name);
+      return;  // Ignore the first run.
+    }
+    timings_[name].push_back(microseconds);
+  }
+
+  TimingStats GetStats(const std::string& name) const {
+    TimingStats stats;
+    auto it = timings_.find(name);
+    if (it == timings_.end()) return stats;
+    const auto& times = it->second;
+    if (times.empty()) return stats;
+    stats.call_count = times.size();
+    stats.min_time_us = *std::min_element(times.begin(), times.end());
+    stats.max_time_us = *std::max_element(times.begin(), times.end());
+    stats.total_time_us = std::accumulate(times.begin(), times.end(), 0.0);
+    // Always use call count from the HydroelasticQuery if available
+    size_t call_count = stats.call_count;
+    auto HydroI = timings_.find("HydroelasticQuery");
+    if (HydroI != timings_.end()) {
+      const auto& HydroTimes = HydroI->second;
+      call_count = HydroTimes.size();
+    }
+    stats.avg_time_us = stats.total_time_us / call_count;
+    return stats;
+  }
+
+  void PrintStats() const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    std::cout << "\n=== CPU Timing Statistics ===" << std::endl;
+    for (const auto& [name, _] : timings_) {
+      auto stats = GetStats(name);
+      std::cout << "Timer: " << name << std::endl;
+      std::cout << "  Calls: " << stats.call_count << std::endl;
+      std::cout << "  Min: " << stats.min_time_us << " μs" << std::endl;
+      std::cout << "  Max: " << stats.max_time_us << " μs" << std::endl;
+      std::cout << "  Avg: " << stats.avg_time_us << " μs" << std::endl;
+      std::cout << "  Total: " << stats.total_time_us << " μs" << std::endl;
+      std::cout << std::endl;
+    }
+  }
+
+  // Print all timing statistics in JSON format
+  void PrintStatsJson(const std::string& path = "") const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    std::ostringstream oss;
+    oss << "{\n";
+    oss << "  \"timings\": {\n";
+    bool first = true;
+    for (const auto& [name, _] : timings_) {
+      if (!first) oss << ",\n";
+      first = false;
+      auto stats = GetStats(name);
+      oss << "    \"" << name << "\": {"
+          << "\"calls\": " << stats.call_count << ", "
+          << "\"min_us\": " << stats.min_time_us << ", "
+          << "\"max_us\": " << stats.max_time_us << ", "
+          << "\"avg_us\": " << stats.avg_time_us << ", "
+          << "\"total_us\": " << stats.total_time_us << "}";
+    }
+    oss << "\n  }\n";
+    oss << "}";
+
+    if (!path.empty()) {
+      std::ofstream ofs(path);
+      if (ofs.is_open()) {
+        ofs << oss.str();
+        ofs.close();
+      } else {
+        std::cerr << "Failed to open file for writing: " << path << std::endl;
+      }
+    } else {
+      std::cout << oss.str() << std::endl;
+    }
+  }
+
+  void Clear() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    timings_.clear();
+  }
+
+  void SetEnabled(bool enabled) { enabled_ = enabled; }
+  bool IsEnabled() const { return enabled_; }
+
+ private:
+  CpuTimingLogger() = default;
+  mutable std::mutex mutex_;
+  std::map<std::string, std::vector<double>> timings_;
+  bool enabled_ = true;
+  std::set<std::string> first_run_seen_;
+};
+
+class ScopedCpuTimer {
+ public:
+  explicit ScopedCpuTimer(const std::string& name)
+      : name_(name), start_(std::chrono::high_resolution_clock::now()) {}
+  ~ScopedCpuTimer() {
+    auto end = std::chrono::high_resolution_clock::now();
+    double duration_us =
+        std::chrono::duration_cast<std::chrono::microseconds>(end - start_)
+            .count();
+    CpuTimingLogger::GetInstance().AddTiming(name_, duration_us);
+  }
+
+ private:
+  std::string name_;
+  std::chrono::high_resolution_clock::time_point start_;
+};
+
+#else  // DRAKE_CPU_TIMING_ENABLED
+
+class CpuTimingLogger {
+ public:
+  struct TimingStats {
+    double min_time_us = 0.0;
+    double max_time_us = 0.0;
+    double avg_time_us = 0.0;
+    double total_time_us = 0.0;
+    size_t call_count = 0;
+  };
+  static CpuTimingLogger& GetInstance() {
+    static CpuTimingLogger instance;
+    return instance;
+  }
+  void AddTiming(const std::string&, double) {}
+  TimingStats GetStats(const std::string&) const { return TimingStats{}; }
+  void PrintStats() const {}
+  void Clear() {}
+  void SetEnabled(bool) {}
+  bool IsEnabled() const { return false; }
+};
+
+class ScopedCpuTimer {
+ public:
+  explicit ScopedCpuTimer(const std::string&) {}
+};
+
+#endif  // DRAKE_CPU_TIMING_ENABLED
+
+// Macro for easy use
+#define DRAKE_CPU_SCOPED_TIMER(name) \
+  drake::common::ScopedCpuTimer timer_##__LINE__(name)
+
+}  // namespace common
+}  // namespace drake