diff --git a/multibody/contact_solvers/BUILD.bazel b/multibody/contact_solvers/BUILD.bazel index c02c8435fcaa..1f9b68d9178c 100644 --- a/multibody/contact_solvers/BUILD.bazel +++ b/multibody/contact_solvers/BUILD.bazel @@ -226,6 +226,7 @@ drake_cc_library( "//common:essential", ], deps = [ + "//common:drake_export", "@conex_internal//conex:supernodal_solver", ], ) diff --git a/multibody/contact_solvers/conex_supernodal_solver.cc b/multibody/contact_solvers/conex_supernodal_solver.cc index 21bad151d531..ee13c5a50f72 100644 --- a/multibody/contact_solvers/conex_supernodal_solver.cc +++ b/multibody/contact_solvers/conex_supernodal_solver.cc @@ -6,6 +6,8 @@ #include "conex/clique_ordering.h" #include "conex/kkt_solver.h" +#include "drake/common/drake_export.h" + using Eigen::MatrixXd; using std::vector; using MatrixBlock = std::pair>; @@ -17,6 +19,18 @@ namespace contact_solvers { namespace internal { namespace { +// Casts the `ConexSuperNodalSolver::solver_` member field to its actual type. +// NOLINTNEXTLINE(runtime/references) +conex::SupernodalKKTSolver& solver_cast(std::shared_ptr& solver) { + DRAKE_DEMAND(solver.get() != nullptr); + return *static_cast(solver.get()); +} +const conex::SupernodalKKTSolver& solver_cast( + const std::shared_ptr& solver) { + DRAKE_DEMAND(solver.get() != nullptr); + return *static_cast(solver.get()); +} + /* Computes the dense matrix Jáµ€*G*J. Matrix J is a column-wise concatenation of entries in `J_blocks`. More specifically, the layout of J looks like @@ -107,7 +121,7 @@ std::vector GetMassMatrixStartingColumn( } // namespace -class ConexSuperNodalSolver::CliqueAssembler final +class DRAKE_NO_EXPORT ConexSuperNodalSolver::CliqueAssembler final : public ::conex::SupernodalAssemblerBase { public: // Helper functions for specifying G_i @@ -362,7 +376,7 @@ void ConexSuperNodalSolver::Initialize( } // Make connections between clique_assemblers and solver->Assemble(). - solver_->Bind(clique_assemblers_ptrs_); + solver_cast(solver_).Bind(clique_assemblers_ptrs_); size_ = mass_matrix_starting_columns.back() + mass_matrices.back().cols(); } @@ -374,7 +388,7 @@ ConexSuperNodalSolver::ConexSuperNodalSolver( SparsityData clique_data = GetEliminationOrdering(num_jacobian_row_blocks, jacobian_blocks); - solver_ = std::make_unique<::conex::SupernodalKKTSolver>( + solver_ = std::make_shared<::conex::SupernodalKKTSolver>( clique_data.variable_cliques, clique_data.data.num_vars, clique_data.data.order, clique_data.data.supernodes, clique_data.data.separators); @@ -412,7 +426,7 @@ bool ConexSuperNodalSolver::DoSetWeightMatrix( } if (weight_matrix_compatible) { - solver_->Assemble(); + solver_cast(solver_).Assemble(); } // Destroy references to argument weight_matrix. @@ -424,16 +438,16 @@ bool ConexSuperNodalSolver::DoSetWeightMatrix( } bool ConexSuperNodalSolver::DoFactor() { - return solver_->Factor(); + return solver_cast(solver_).Factor(); } void ConexSuperNodalSolver::DoSolveInPlace(Eigen::VectorXd* b) const { Eigen::Map ymap(b->data(), b->rows(), 1); - solver_->SolveInPlace(&ymap); + solver_cast(solver_).SolveInPlace(&ymap); } Eigen::MatrixXd ConexSuperNodalSolver::DoMakeFullMatrix() const { - return solver_->KKTMatrix(); + return solver_cast(solver_).KKTMatrix(); } void ConexSuperNodalSolver::CliqueAssembler::Initialize( diff --git a/multibody/contact_solvers/conex_supernodal_solver.h b/multibody/contact_solvers/conex_supernodal_solver.h index aeb41fb3997f..906ac5a72890 100644 --- a/multibody/contact_solvers/conex_supernodal_solver.h +++ b/multibody/contact_solvers/conex_supernodal_solver.h @@ -8,14 +8,6 @@ #include "drake/common/drake_copyable.h" #include "drake/multibody/contact_solvers/supernodal_solver.h" -#ifndef DRAKE_DOXYGEN_CXX -// Forward declaration to avoid the inclusion of conex's headers within a Drake -// header. -namespace conex { -class SupernodalKKTSolver; -} -#endif - namespace drake { namespace multibody { namespace contact_solvers { @@ -78,7 +70,12 @@ class ConexSuperNodalSolver final : public SuperNodalSolver { const std::vector& jacobian_blocks, const std::vector& mass_matrices); - std::unique_ptr<::conex::SupernodalKKTSolver> solver_; + // We use 'void' here to avoid the inclusion of conex's headers within a + // Drake header; the actual type of the object is a SupernodalKKTSolver. + // We use a shared_ptr to have a type-erased deleter; the object is not + // actually ever shared anywhere. + std::shared_ptr solver_; + // N.B. This array stores pointers to clique assemblers owned by // owned_clique_assemblers_. std::vector clique_assemblers_ptrs_; diff --git a/tools/workspace/BUILD.bazel b/tools/workspace/BUILD.bazel index eac3d25b93d3..f091cc1a984c 100644 --- a/tools/workspace/BUILD.bazel +++ b/tools/workspace/BUILD.bazel @@ -46,16 +46,17 @@ drake_py_binary( name = "vendor_cxx", srcs = ["vendor_cxx.py"], visibility = [ - # These should all be of the form "@foo_internal//:__pkg__". - "@fcl_internal//:__pkg__", - "@gz_math_internal//:__pkg__", - "@gz_utils_internal//:__pkg__", - "@ipopt_internal_fromsource//:__pkg__", - "@msgpack_internal//:__pkg__", - "@nlopt_internal//:__pkg__", - "@qhull_internal//:__pkg__", - "@sdformat_internal//:__pkg__", - "@yaml_cpp_internal//:__pkg__", + # These should all be of the form "@foo_internal//:__subpackages__". + "@conex_internal//:__subpackages__", + "@fcl_internal//:__subpackages__", + "@gz_math_internal//:__subpackages__", + "@gz_utils_internal//:__subpackages__", + "@ipopt_internal_fromsource//:__subpackages__", + "@msgpack_internal//:__subpackages__", + "@nlopt_internal//:__subpackages__", + "@qhull_internal//:__subpackages__", + "@sdformat_internal//:__subpackages__", + "@yaml_cpp_internal//:__subpackages__", ], deps = [":module_py"], ) diff --git a/tools/workspace/conex_internal/patches/eigen_namespace_pollution.patch b/tools/workspace/conex_internal/patches/eigen_namespace_pollution.patch new file mode 100644 index 000000000000..1f70e10de058 --- /dev/null +++ b/tools/workspace/conex_internal/patches/eigen_namespace_pollution.patch @@ -0,0 +1,178 @@ +[conex] Fix RLDLT to live in the conex namespace + +Projects must not define classes in another project's namespace. +When we fork code from another project, we need to switch it to +our own namespace. + +This is a copy of https://github.com/ToyotaResearchInstitute/conex/pull/6. + +--- conex/RLDLT.h ++++ conex/RLDLT.h +@@ -1,6 +1,6 @@ +-#include "conex/debug_macros.h" +-#include +-// This file is part of Eigen, a lightweight C++ template library ++#pragma once ++ ++// This file was copied from Eigen, a lightweight C++ template library + // for linear algebra. + // + // Copyright (C) 2008-2011 Gael Guennebaud +@@ -12,19 +12,20 @@ + // Public License v. 2.0. If a copy of the MPL was not distributed + // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +-#ifndef EIGEN_RLDLT_H +-#define EIGEN_RLDLT_H ++#include ++ ++#include "conex/debug_macros.h" ++ ++namespace conex { ++namespace eigen_stuff { + +-namespace Eigen { ++using namespace Eigen; ++namespace internal = Eigen::internal; + +-namespace internal { ++namespace detail { + template + struct RLDLT_Traits; +- +-// PositiveSemiDef means positive semi-definite and non-zero; same for +-// NegativeSemiDef +-// enum SignMatrix { PositiveSemiDef, NegativeSemiDef, ZeroSign, Indefinite }; +-} // namespace internal ++} // namespace detail + + /** \ingroup Cholesky_Module + * +@@ -78,7 +79,7 @@ class RLDLT { + typedef PermutationMatrix + PermutationType; + +- typedef internal::LDLT_Traits Traits; ++ typedef detail::RLDLT_Traits Traits; + + /** \brief Default Constructor. + * +@@ -289,7 +290,9 @@ class RLDLT { + ComputationInfo m_info; + }; + +-namespace internal { ++namespace detail { ++ ++using namespace Eigen::internal; + + template + struct rldlt_inplace; +@@ -564,7 +567,7 @@ RLDLT& RLDLT::compute( + m_temporary.resize(size); + m_sign = internal::ZeroSign; + +- m_regularization_used = !internal::rldlt_inplace::unblocked( ++ m_regularization_used = !detail::rldlt_inplace::unblocked( + m_matrix, m_transpositions, m_temporary, m_sign); + + m_info = Success; +@@ -598,7 +601,7 @@ RLDLT& RLDLT::rankUpdate( + m_isInitialized = true; + } + +- internal::rldlt_inplace::update(m_matrix, m_transpositions, m_temporary, ++ detail::rldlt_inplace::update(m_matrix, m_transpositions, m_temporary, + w, sigma); + + return *this; +@@ -696,6 +699,5 @@ MatrixType RLDLT::reconstructedMatrix() const { + return res; + } + +-} // end namespace Eigen +- +-#endif // EIGEN_RLDLT_H ++} // namespace eigen_stuff ++} // namespace conex +--- conex/block_triangular_operations.cc ++++ conex/block_triangular_operations.cc +@@ -221,7 +221,7 @@ bool T::BlockCholeskyInPlace(TriangularMatrixWorkspace* C) { + // Apply inv(M^T) = inv(L^T P) = P^T inv(L^T) + void T::ApplyBlockInverseOfMTranspose( + const TriangularMatrixWorkspace& mat, +- const std::vector>> factorization, ++ const std::vector>> factorization, + VectorXd* y) { + PartitionVectorIterator ypart(*y, mat.N, mat.supernode_size); + // mat.diagonal.back().triangularView().transpose().solveInPlace(ypart.b_i()); +@@ -264,7 +264,7 @@ void T::ApplyBlockInverseOfMTranspose( + + void T::ApplyBlockInverseOfMD( + const TriangularMatrixWorkspace& mat, +- const std::vector>> factorization, ++ const std::vector>> factorization, + VectorXd* y) { + // Apply inv(M) = inv(P^T L) = inv(L) P + PartitionVectorForwardIterator ypart(*y, mat.supernode_size); +@@ -314,7 +314,7 @@ void T::ApplyBlockInverseOfMD( + // = inv(D_1) inv(L) P * off_diag + bool T::BlockLDLTInPlace( + TriangularMatrixWorkspace* C, +- std::vector>>* factorization) { ++ std::vector>>* factorization) { + auto& llts = *factorization; + llts.clear(); + +--- conex/block_triangular_operations.h ++++ conex/block_triangular_operations.h +@@ -14,22 +14,22 @@ struct BlockTriangularOperations { + static bool BlockCholeskyInPlace(TriangularMatrixWorkspace* mat); + static bool BlockLDLTInPlace( + TriangularMatrixWorkspace* mat, +- std::vector>>* factorization); ++ std::vector>>* factorization); + static void ApplyBlockInverseOfMTranspose( + const TriangularMatrixWorkspace& mat, +- const std::vector>> ++ const std::vector>> + factorization, + Eigen::VectorXd* y); + + static void ApplyBlockInverseOfMD( + const TriangularMatrixWorkspace& mat, +- const std::vector>> ++ const std::vector>> + factorization, + Eigen::VectorXd* y); + + static void SolveInPlaceLDLT( + const TriangularMatrixWorkspace& mat, +- const std::vector>> ++ const std::vector>> + factorization, + Eigen::VectorXd* y) { + ApplyBlockInverseOfMD(mat, factorization, y); +--- conex/kkt_solver.h ++++ conex/kkt_solver.h +@@ -53,7 +53,7 @@ class SupernodalKKTSolver { + const std::vector> dual_variables_; + MatrixData data; + SparseTriangularMatrix mat; +- std::vector>> factorization; ++ std::vector>> factorization; + Eigen::PermutationMatrix<-1> Pt; + mutable Eigen::VectorXd b_permuted_; + std::vector assembler; +index 1277cff..1711d5b 100644 +--- conex/test/block_triangular_operations_test.cc ++++ conex/test/block_triangular_operations_test.cc +@@ -202,7 +202,7 @@ void DoLDLTTest(bool diagonal, const std::vector& cliques) { + + Eigen::MatrixXd X = T::ToDense(mat).selfadjointView(); + +- std::vector>> factorization; ++ std::vector>> factorization; + B::BlockLDLTInPlace(&mat.workspace_, &factorization); + + Eigen::VectorXd z = Eigen::VectorXd::Random(X.cols()); diff --git a/tools/workspace/conex_internal/patches/vendor.patch b/tools/workspace/conex_internal/patches/vendor.patch new file mode 100644 index 000000000000..d338e6faadf9 --- /dev/null +++ b/tools/workspace/conex_internal/patches/vendor.patch @@ -0,0 +1,13 @@ +[conex] Use a custom cc_library rule for vendoring + +--- conex/BUILD ++++ conex/BUILD +@@ -1,3 +1,8 @@ ++load( ++ "@drake//tools/workspace/conex_internal:vendor.bzl", ++ cc_library = "conex_cc_library", ++) ++ + cc_library( + name = "test_util", + srcs = glob(["test/test_util.cc"]), diff --git a/tools/workspace/conex_internal/repository.bzl b/tools/workspace/conex_internal/repository.bzl index d2b5f629c723..e1a88638e7a0 100644 --- a/tools/workspace/conex_internal/repository.bzl +++ b/tools/workspace/conex_internal/repository.bzl @@ -11,7 +11,9 @@ def conex_internal_repository( build_file = ":package.BUILD.bazel", patches = [ ":patches/debug_macros.patch", + ":patches/eigen_namespace_pollution.patch", ":patches/no_eigen_io.patch", + ":patches/vendor.patch", ], mirrors = mirrors, ) diff --git a/tools/workspace/conex_internal/vendor.bzl b/tools/workspace/conex_internal/vendor.bzl new file mode 100644 index 000000000000..e77c3e52a6f6 --- /dev/null +++ b/tools/workspace/conex_internal/vendor.bzl @@ -0,0 +1,66 @@ +load("//tools/workspace:vendor_cxx.bzl", "cc_library_vendored") + +def conex_cc_library( + *, + name, + srcs = None, + hdrs = None, + copts = None, + deps = None, + visibility = None): + """For anything not named "supernodal_solver", creates an empty cc_library + with the given name (to placate Bazel). + + For "supernodal_solver", declares a cc_library in the usual sense, except + rewrites the source code so that it uses a `drake_vendor` inline hidden + namespace wrapper so that the library's linker symbols are private. For + this, we can delegate to Drake's existing `cc_library_vendored` rule, after + adjusting for a few BUILD file typos upstream. + """ + + # Only vendor the one library we care about. Otherwise, just emit an empty + # library to placate Bazel. + if name != "supernodal_solver": + native.cc_library(name = name, visibility = visibility) + return + + # Switch one of the `deps` to be source files instead, for simplicity. + srcs = list(srcs) + hdrs = list(hdrs) + deps = list(deps) + deps.remove("tree_utils") + srcs.append("tree_utils.cc") + hdrs.append("tree_utils.h") + + # Fix some hdrs that are mis-identified as srcs upstream. + for hdr in [x for x in srcs if x.endswith(".h")]: + srcs.remove(hdr) + if hdr not in hdrs: + hdrs.append(hdr) + + # Choose where the vendored files will live. + hdrs_vendored = [ + "drake_vendor/conex/" + x + for x in hdrs or [] + ] + srcs_vendored = [ + "drake_vendor/conex/" + x + for x in srcs or [] + ] + edit_include = None + strip_include_prefix = "drake_vendor" + + # Compile the static library. + cc_library_vendored( + name = name, + srcs = srcs, + srcs_vendored = srcs_vendored, + hdrs = hdrs, + hdrs_vendored = hdrs_vendored, + edit_include = edit_include, + strip_include_prefix = strip_include_prefix, + copts = copts, + deps = deps, + visibility = visibility, + linkstatic = True, + )