diff --git a/solvers/moby_lcp_solver.cc b/solvers/moby_lcp_solver.cc index a7f987793895..e213e5580c2c 100644 --- a/solvers/moby_lcp_solver.cc +++ b/solvers/moby_lcp_solver.cc @@ -1,24 +1,21 @@ -// TODO(jwnimmer-tri) Rewrite the logging in this file to use spdlog. -#undef EIGEN_NO_IO - #include "drake/solvers/moby_lcp_solver.h" #include #include #include -#include #include #include -#include #include #include #include #include #include +#include #include #include "drake/common/drake_assert.h" +#include "drake/common/fmt_ostream.h" #include "drake/common/never_destroyed.h" #include "drake/common/text_logging.h" @@ -133,19 +130,6 @@ Eigen::Index minCoeffIdx(const Eigen::MatrixBase& in) { const double kSqrtEps = std::sqrt(std::numeric_limits::epsilon()); } // namespace -template -void MobyLCPSolver::SetLoggingEnabled(bool enabled) { - log_enabled_ = enabled; -} - -template -std::ostream& MobyLCPSolver::Log() const { - if (log_enabled_) { - return std::cerr; - } - return null_stream_; -} - template void MobyLCPSolver::ClearIndexVectors() const { // clear all vectors @@ -233,11 +217,11 @@ bool MobyLCPSolver::SolveLcpFast(const MatrixX& M, const VectorX& q, if (M.rows() != N || M.cols() != N) throw std::logic_error("M's dimensions do not match that of q."); - Log() << "MobyLCPSolver::SolveLcpFast() entered" << std::endl; + log()->debug("MobyLCPSolver::SolveLcpFast() entered"); // look for trivial solution if (N == 0) { - Log() << "MobyLCPSolver::SolveLcpFast() - empty problem" << std::endl; + log()->debug("MobyLCPSolver::SolveLcpFast() - empty problem"); z->resize(0); return true; } @@ -252,8 +236,7 @@ bool MobyLCPSolver::SolveLcpFast(const MatrixX& M, const VectorX& q, // see whether to warm-start if (z->size() == q.size()) { - Log() << "MobyLCPSolver::SolveLcpFast() - warm starting activated" - << std::endl; + log()->debug("MobyLCPSolver::SolveLcpFast() - warm starting activated"); for (unsigned i = 0; i < z->size(); i++) { if (abs((*z)[i]) < mod_zero_tol) { @@ -263,19 +246,13 @@ bool MobyLCPSolver::SolveLcpFast(const MatrixX& M, const VectorX& q, } } - if (log_enabled_) { - std::ostringstream str; - str << " -- non-basic indices:"; - for (unsigned i = 0; i < nonbas_.size(); i++) str << " " << nonbas_[i]; - Log() << str.str() << std::endl; - } + log()->debug(" -- non-basic indices: {}", fmt::join(nonbas_, " ")); } else { // get minimum element of q (really w) Eigen::Index minw; const T minw_val = q.minCoeff(&minw); if (minw_val > -mod_zero_tol) { - Log() << "MobyLCPSolver::SolveLcpFast() - trivial solution found" - << std::endl; + log()->debug("MobyLCPSolver::SolveLcpFast() - trivial solution found"); z->resize(N); z->fill(0); return true; @@ -324,9 +301,10 @@ bool MobyLCPSolver::SolveLcpFast(const MatrixX& M, const VectorX& q, if (minw == UINF || w[minw] > -mod_zero_tol) { // find the (a) minimum of z unsigned minz = (zz.rows() > 0) ? minCoeffIdx(zz) : UINF; - if (log_enabled_ && zz.rows() > 0) { - Log() << "MobyLCPSolver::SolveLcpFast() - minimum z after pivot: " - << zz[minz] << std::endl; + if (zz.rows() > 0) { + log()->debug( + "MobyLCPSolver::SolveLcpFast() - minimum z after pivot: {}", + zz[minz]); } if (minz < UINF && zz[minz] < -mod_zero_tol) { // get the original index and remove it from the nonbasic set @@ -346,11 +324,11 @@ bool MobyLCPSolver::SolveLcpFast(const MatrixX& M, const VectorX& q, (*z)[nonbas_[j]] = zz[i]; } - Log() << "MobyLCPSolver::SolveLcpFast() - solution found!" << std::endl; + log()->debug("MobyLCPSolver::SolveLcpFast() - solution found!"); return true; } } else { - Log() << "(minimum w too negative)" << std::endl; + log()->debug("(minimum w too negative)"); // one or more components of w violating w >= 0 // move component of w from basic set to nonbasic set @@ -361,15 +339,16 @@ bool MobyLCPSolver::SolveLcpFast(const MatrixX& M, const VectorX& q, // look whether any component of z needs to move to basic set unsigned minz = (zz.rows() > 0) ? minCoeffIdx(zz) : UINF; - if (log_enabled_ && zz.rows() > 0) { - Log() << "MobyLCPSolver::SolveLcpFast() - minimum z after pivot: " - << zz[minz] << std::endl; + if (zz.rows() > 0) { + log()->debug( + "MobyLCPSolver::SolveLcpFast() - minimum z after pivot: {}", + zz[minz]); } if (minz < UINF && zz[minz] < -mod_zero_tol) { // move index to basic set and continue looping unsigned k = nonbas_[minz]; - Log() << "MobyLCPSolver::SolveLcpFast() - moving index " << k - << " to basic set" << std::endl; + log()->debug( + "MobyLCPSolver::SolveLcpFast() - moving index {} to basic set", k); nonbas_.erase(nonbas_.begin() + minz); bas_.push_back(k); @@ -378,8 +357,8 @@ bool MobyLCPSolver::SolveLcpFast(const MatrixX& M, const VectorX& q, } } - Log() << "MobyLCPSolver::SolveLcpFast() - maximum allowable pivots exceeded" - << std::endl; + log()->debug( + "MobyLCPSolver::SolveLcpFast() - maximum allowable pivots exceeded"); // if we're here, then the maximum number of pivots has been exceeded z->setZero(N); @@ -392,7 +371,7 @@ bool MobyLCPSolver::SolveLcpFastRegularized(const MatrixX& M, VectorX* z, int min_exp, unsigned step_exp, int max_exp, const T& zero_tol) const { - Log() << "MobyLCPSolver::SolveLcpFastRegularized() entered" << std::endl; + log()->debug("MobyLCPSolver::SolveLcpFastRegularized() entered"); // Variables that will be reused multiple times, thus hopefully allowing // Eigen to keep from freeing/reallocating memory repeatedly. @@ -429,7 +408,7 @@ bool MobyLCPSolver::SolveLcpFastRegularized(const MatrixX& M, // Assign value for zero tolerance, if necessary. const T mod_zero_tol = (zero_tol > 0) ? zero_tol : ComputeZeroTolerance(M); - Log() << " zero tolerance: " << mod_zero_tol << std::endl; + log()->debug(" zero tolerance: {}", mod_zero_tol); // store the total pivots unsigned total_piv = 0; @@ -448,31 +427,32 @@ bool MobyLCPSolver::SolveLcpFastRegularized(const MatrixX& M, const T wx_max = wx.maxCoeff(); if (wx_min >= -mod_zero_tol && wx_max < mod_zero_tol) { - Log() << " solved with no regularization necessary!" << std::endl; - Log() << " pivots / total pivots: " << pivots_ << " " << pivots_ - << std::endl; - Log() << "MobyLCPSolver::SolveLcpFastRegularized() exited" - << std::endl; - + log()->debug(" solved with no regularization necessary!"); + log()->debug(" pivots / total pivots: {} {}", pivots_, pivots_); + log()->debug("MobyLCPSolver::SolveLcpFastRegularized() exited"); return true; } else { - Log() << "MobyLCPSolver::SolveLcpFastRegularized() - " - << "' not within tolerance(min value: " << wx_min - << " max value: " << wx_max << ")" << std::endl; + log()->debug( + "MobyLCPSolver::SolveLcpFastRegularized() - " + "' not within tolerance(min value: {} max value: {})", + wx_min, wx_max); } } else { - Log() << " MobyLCPSolver::SolveLcpFastRegularized() - " - << "'w' not solved to desired tolerance" << std::endl; - Log() << " minimum w: " << wx.minCoeff() << std::endl; + log()->debug( + " MobyLCPSolver::SolveLcpFastRegularized() - " + "'w' not solved to desired tolerance"); + log()->debug(" minimum w: {}", wx.minCoeff()); } } else { - Log() << " MobyLCPSolver::SolveLcpFastRegularized() - " - << "'z' not solved to desired tolerance" << std::endl; - Log() << " minimum z: " << z->minCoeff() << std::endl; + log()->debug( + " MobyLCPSolver::SolveLcpFastRegularized() - " + "'z' not solved to desired tolerance"); + log()->debug(" minimum z: {}", z->minCoeff()); } } else { - Log() << " MobyLCPSolver::SolveLcpFastRegularized() " - << "- solver failed with zero regularization" << std::endl; + log()->debug( + " MobyLCPSolver::SolveLcpFastRegularized() " + "- solver failed with zero regularization"); } // update the pivots @@ -485,8 +465,8 @@ bool MobyLCPSolver::SolveLcpFastRegularized(const MatrixX& M, double lambda = std::pow(static_cast(10.0), static_cast(rf)); - Log() << " trying to solve LCP with regularization factor: " << lambda - << std::endl; + log()->debug(" trying to solve LCP with regularization factor: {}", + lambda); // regularize M MM = M; @@ -512,28 +492,28 @@ bool MobyLCPSolver::SolveLcpFastRegularized(const MatrixX& M, const T wx_max = wx.maxCoeff(); if (wx_min > -mod_zero_tol && wx_max < mod_zero_tol) { - Log() << " solved with regularization factor: " << lambda - << std::endl; - Log() << " pivots / total pivots: " << pivots_ << " " << total_piv - << std::endl; - Log() << "MobyLCPSolver::SolveLcpFastRegularized() exited" - << std::endl; + log()->debug(" solved with regularization factor: {}", lambda); + log()->debug(" pivots / total pivots: {} {}", pivots_, total_piv); + log()->debug("MobyLCPSolver::SolveLcpFastRegularized() exited"); pivots_ = total_piv; return true; } else { - Log() << "MobyLCPSolver::SolveLcpFastRegularized() - " - << "' not within tolerance(min value: " << wx_min - << " max value: " << wx_max << ")" << std::endl; + log()->debug( + "MobyLCPSolver::SolveLcpFastRegularized() - " + "' not within tolerance(min value: {} max value: {})", + wx_min, wx_max); } } else { - Log() << " MobyLCPSolver::SolveLcpFastRegularized() - " - << "'w' not solved to desired tolerance" << std::endl; - Log() << " minimum w: " << wx.minCoeff() << std::endl; + log()->debug( + " MobyLCPSolver::SolveLcpFastRegularized() - " + "'w' not solved to desired tolerance"); + log()->debug(" minimum w: {}", wx.minCoeff()); } } else { - Log() << " MobyLCPSolver::SolveLcpFastRegularized() - " - << "'z' not solved to desired tolerance" << std::endl; - Log() << " minimum z: " << z->minCoeff() << std::endl; + log()->debug( + " MobyLCPSolver::SolveLcpFastRegularized() - " + "'z' not solved to desired tolerance"); + log()->debug(" minimum z: {}", z->minCoeff()); } } @@ -541,8 +521,8 @@ bool MobyLCPSolver::SolveLcpFastRegularized(const MatrixX& M, rf += step_exp; } - Log() << " unable to solve given any regularization!" << std::endl; - Log() << "MobyLCPSolver::SolveLcpFastRegularized() exited" << std::endl; + log()->debug(" unable to solve given any regularization!"); + log()->debug("MobyLCPSolver::SolveLcpFastRegularized() exited"); // store total pivots pivots_ = total_piv; @@ -577,15 +557,11 @@ void MobyLCPSolver::FinishLemkeSolution(const MatrixType& M, z->conservativeResize(q.size()); // check to see whether tolerances are satisfied - if (log_enabled_) { - VectorX wl = (M * (*z)) + q; - const T minw = wl.minCoeff(); - const T w_dot_z = abs(wl.dot(*z)); - Log() << " z: " << z << std::endl; - Log() << " w: " << wl << std::endl; - Log() << " minimum w: " << minw << std::endl; - Log() << " w'z: " << w_dot_z << std::endl; - } + const VectorX wl = (M * (*z)) + q; + log()->debug(" z: {}", fmt_eigen(*z)); + log()->debug(" w: {}", fmt_eigen(wl)); + log()->debug(" minimum w: {}", wl.minCoeff()); + log()->debug(" w'z: {}", T{abs(wl.dot(*z))}); } template @@ -599,11 +575,9 @@ bool MobyLCPSolver::SolveLcpLemke(const MatrixX& M, const VectorX& q, VectorX result, dj, dl, x, xj, Be, u, z0; MatrixX Bl, t1, t2; - if (log_enabled_) { - Log() << "MobyLCPSolver::SolveLcpLemke() entered" << std::endl; - Log() << " M: " << std::endl << M; - Log() << " q: " << q << std::endl; - } + log()->debug("MobyLCPSolver::SolveLcpLemke() entered"); + log()->debug(" M: {}", fmt_eigen(M)); + log()->debug(" q: {}", fmt_eigen(q)); const unsigned n = q.size(); const unsigned max_iter = std::min(unsigned{1000}, 50 * n); @@ -625,8 +599,8 @@ bool MobyLCPSolver::SolveLcpLemke(const MatrixX& M, const VectorX& q, if (mod_zero_tol <= 0) mod_zero_tol = ComputeZeroTolerance(M); if (CheckLemkeTrivial(n, mod_zero_tol, q, z)) { - Log() << " -- trivial solution found" << std::endl; - Log() << "MobyLCPSolver::SolveLcpLemke() exited" << std::endl; + log()->debug(" -- trivial solution found"); + log()->debug("MobyLCPSolver::SolveLcpLemke() exited"); return true; } @@ -670,7 +644,7 @@ bool MobyLCPSolver::SolveLcpLemke(const MatrixX& M, const VectorX& q, // determine initial values if (!bas_.empty()) { - Log() << "-- initial basis not empty (warmstarting)" << std::endl; + log()->debug("-- initial basis not empty (warmstarting)"); // start from good initial basis Bl.resize(n, n); @@ -691,7 +665,7 @@ bool MobyLCPSolver::SolveLcpLemke(const MatrixX& M, const VectorX& q, // Solve B*x = -q. x = LinearSolve(Bl, q); } else { - Log() << "-- using basis of -1 (no warmstarting)" << std::endl; + log()->debug("-- using basis of -1 (no warmstarting)"); // use standard initial basis Bl.resize(n, n); @@ -702,9 +676,9 @@ bool MobyLCPSolver::SolveLcpLemke(const MatrixX& M, const VectorX& q, // check whether initial basis provides a solution if (x.minCoeff() >= 0.0) { - Log() << " -- initial basis provides a solution!" << std::endl; + log()->debug(" -- initial basis provides a solution!"); FinishLemkeSolution(M, q, x, z); - Log() << "MobyLCPSolver::SolveLcpLemke() exited" << std::endl; + log()->debug("MobyLCPSolver::SolveLcpLemke() exited"); return true; } @@ -724,10 +698,11 @@ bool MobyLCPSolver::SolveLcpLemke(const MatrixX& M, const VectorX& q, iiter = bas_.begin(); std::advance(iiter, lvindex); leaving = *iiter; - Log() << " -- x: " << x << std::endl; - Log() << " -- first pivot: leaving index=" << lvindex - << " entering index=" << entering << " minimum value: " << tval - << std::endl; + log()->debug(" -- x: {}", fmt_eigen(x)); + log()->debug( + " -- first pivot: leaving index={} entering index={} " + "minimum value: {}", + lvindex, entering, tval); // pivot in the artificial variable *iiter = t; // replace w var with _z0 in basic indices @@ -740,24 +715,18 @@ bool MobyLCPSolver::SolveLcpLemke(const MatrixX& M, const VectorX& q, x += u; x[lvindex] = tval; Bl.col(lvindex) = Be; - Log() << " new q: " << x << std::endl; + log()->debug(" new q: {}", fmt_eigen(x)); // main iterations begin here for (pivots_ = 0; pivots_ < max_iter; pivots_++) { - if (log_enabled_) { - std::ostringstream basic; - for (unsigned i = 0; i < bas_.size(); i++) { - basic << " " << bas_[i]; - } - Log() << "basic variables:" << basic.str() << std::endl; - Log() << "leaving: " << leaving << " t:" << t << std::endl; - } + log()->debug("basic variables: {}", fmt::join(bas_, " ")); + log()->debug("leaving: {} t: {}", leaving, t); // check whether done; if not, get new entering variable if (leaving == t) { - Log() << "-- solved LCP successfully!" << std::endl; + log()->debug("-- solved LCP successfully!"); FinishLemkeSolution(M, q, x, z); - Log() << "MobyLCPSolver::SolveLcpLemke() exited" << std::endl; + log()->debug("MobyLCPSolver::SolveLcpLemke() exited"); return true; } else if (leaving < n) { entering = n + leaving; @@ -783,20 +752,15 @@ bool MobyLCPSolver::SolveLcpLemke(const MatrixX& M, const VectorX& q, // check for no new pivots; ray termination if (j_.empty()) { - Log() - << "MobyLCPSolver::SolveLcpLemke() - no new pivots (ray termination)" - << std::endl; - Log() << "MobyLCPSolver::SolveLcpLemke() exiting" << std::endl; + log()->debug( + "MobyLCPSolver::SolveLcpLemke() - no new pivots (ray termination)"); + log()->debug("MobyLCPSolver::SolveLcpLemke() exiting"); z->setZero(n); return false; } - if (log_enabled_) { - std::ostringstream j; - for (unsigned i = 0; i < j_.size(); i++) j << " " << j_[i]; - Log() << "d: " << dl << std::endl; - Log() << "j (before min ratio):" << j.str() << std::endl; - } + log()->debug("d: {}", fmt_eigen(dl)); + log()->debug("j (before min ratio): {}", fmt::join(j_, " ")); // select elements j from x and d selectSubVec(x, j_, &xj); @@ -830,18 +794,12 @@ bool MobyLCPSolver::SolveLcpLemke(const MatrixX& M, const VectorX& q, iiter = j_.erase(iiter); } } - if (log_enabled_) { - std::ostringstream j; - for (unsigned i = 0; i < j_.size(); i++) { - j << " " << j_[i]; - } - Log() << "j (after min ratio):" << j.str() << std::endl; - } + log()->debug("j (after min ratio): {}", fmt::join(j_, " ")); // if j is empty, then likely the zero tolerance is too low if (j_.empty()) { - Log() << "zero tolerance too low?" << std::endl; - Log() << "MobyLCPSolver::SolveLcpLemke() exited" << std::endl; + log()->debug("zero tolerance too low?"); + log()->debug("MobyLCPSolver::SolveLcpLemke() exited"); z->setZero(n); return false; } @@ -875,13 +833,13 @@ bool MobyLCPSolver::SolveLcpLemke(const MatrixX& M, const VectorX& q, x[lvindex] = ratio; Bl.col(lvindex) = Be; *iiter = entering; - Log() << " -- pivoting: leaving index=" << lvindex - << " entering index=" << entering << std::endl; + log()->debug(" -- pivoting: leaving index={} entering index={}", lvindex, + entering); } - Log() << " -- maximum number of iterations exceeded (n=" << n - << ", max=" << max_iter << ")" << std::endl; - Log() << "MobyLCPSolver::SolveLcpLemke() exited" << std::endl; + log()->debug(" -- maximum number of iterations exceeded (n={}, max={})", n, + max_iter); + log()->debug("MobyLCPSolver::SolveLcpLemke() exited"); z->setZero(n); return false; } @@ -894,7 +852,7 @@ bool MobyLCPSolver::SolveLcpLemkeRegularized( // Eigen to keep from freeing/reallocating memory repeatedly. VectorX wx; - Log() << "MobyLCPSolver::SolveLcpLemkeRegularized() entered" << std::endl; + log()->debug("MobyLCPSolver::SolveLcpLemkeRegularized() entered"); // look for fast exit if (q.size() == 0) { @@ -910,7 +868,7 @@ bool MobyLCPSolver::SolveLcpLemkeRegularized( // rather than for each regularized version of M. const T mod_zero_tol = (zero_tol > 0) ? zero_tol : ComputeZeroTolerance(M); - Log() << " zero tolerance: " << mod_zero_tol << std::endl; + log()->debug(" zero tolerance: {}", mod_zero_tol); // store the total pivots unsigned total_piv = 0; @@ -929,27 +887,26 @@ bool MobyLCPSolver::SolveLcpLemkeRegularized( const T wx_min = wx.minCoeff(); const T wx_max = wx.maxCoeff(); if (wx_min >= -mod_zero_tol && wx_max < mod_zero_tol) { - Log() << " solved with no regularization necessary!" << std::endl; - Log() << "MobyLCPSolver::SolveLcpLemkeRegularized() exited" - << std::endl; - + log()->debug(" solved with no regularization necessary!"); + log()->debug("MobyLCPSolver::SolveLcpLemkeRegularized() exited"); return true; } else { - Log() << "MobyLCPSolver::SolveLcpLemke() - " - << "' not within tolerance(min value: " << wx_min - << " max value: " << wx_max << ")" << std::endl; + log()->debug( + "MobyLCPSolver::SolveLcpLemke() - " + "' not within tolerance(min value: {} max value: {})", + wx_min, wx_max); } } else { - Log() << " MobyLCPSolver::SolveLcpLemke() - 'w' not solved to desired " - "tolerance" - << std::endl; - Log() << " minimum w: " << wx.minCoeff() << std::endl; + log()->debug( + " MobyLCPSolver::SolveLcpLemke() - 'w' not solved to desired " + "tolerance"); + log()->debug(" minimum w: {}", wx.minCoeff()); } } else { - Log() << " MobyLCPSolver::SolveLcpLemke() - 'z' not solved to desired " - "tolerance" - << std::endl; - Log() << " minimum z: " << z->minCoeff() << std::endl; + log()->debug( + " MobyLCPSolver::SolveLcpLemke() - 'z' not solved to desired " + "tolerance"); + log()->debug(" minimum z: {}", z->minCoeff()); } } @@ -963,8 +920,8 @@ bool MobyLCPSolver::SolveLcpLemkeRegularized( double lambda = std::pow(static_cast(10.0), static_cast(rf)); - Log() << " trying to solve LCP with regularization factor: " << lambda - << std::endl; + log()->debug(" trying to solve LCP with regularization factor: {}", + lambda); // regularize M MM = M; @@ -990,28 +947,27 @@ bool MobyLCPSolver::SolveLcpLemkeRegularized( const T wx_min = wx.minCoeff(); const T wx_max = wx.maxCoeff(); if (wx_min > -mod_zero_tol && wx_max < mod_zero_tol) { - Log() << " solved with regularization factor: " << lambda - << std::endl; - Log() << "MobyLCPSolver::SolveLcpLemkeRegularized() exited" - << std::endl; + log()->debug(" solved with regularization factor: {}", lambda); + log()->debug("MobyLCPSolver::SolveLcpLemkeRegularized() exited"); pivots_ = total_piv; return true; } else { - Log() << "MobyLCPSolver::SolveLcpLemke() - " - << "' not within tolerance(min value: " << wx_min - << " max value: " << wx_max << ")" << std::endl; + log()->debug( + "MobyLCPSolver::SolveLcpLemke() - " + "' not within tolerance(min value: {} max value: {})", + wx_min, wx_max); } } else { - Log() << " MobyLCPSolver::SolveLcpLemke() - 'w' not solved to " - "desired tolerance" - << std::endl; - Log() << " minimum w: " << wx.minCoeff() << std::endl; + log()->debug( + " MobyLCPSolver::SolveLcpLemke() - 'w' not solved to " + "desired tolerance"); + log()->debug(" minimum w: {}", wx.minCoeff()); } } else { - Log() << " MobyLCPSolver::SolveLcpLemke() - 'z' not solved to desired " - "tolerance" - << std::endl; - Log() << " minimum z: " << z->minCoeff() << std::endl; + log()->debug( + " MobyLCPSolver::SolveLcpLemke() - 'z' not solved to desired " + "tolerance"); + log()->debug(" minimum z: {}", z->minCoeff()); } } @@ -1019,8 +975,8 @@ bool MobyLCPSolver::SolveLcpLemkeRegularized( rf += step_exp; } - Log() << " unable to solve given any regularization!" << std::endl; - Log() << "MobyLCPSolver::SolveLcpLemkeRegularized() exited" << std::endl; + log()->debug(" unable to solve given any regularization!"); + log()->debug("MobyLCPSolver::SolveLcpLemkeRegularized() exited"); // store total pivots pivots_ = total_piv; @@ -1101,9 +1057,18 @@ bool MobyLCPSolver::ProgramAttributesSatisfied( return true; } -// Instantiate templates. -template class MobyLCPSolver; -template class MobyLCPSolver>; - } // namespace solvers } // namespace drake + +using AutoDiff1d = Eigen::AutoDiffScalar; + +// Provide a formatter for Moby's irregular AutoDiff type, so that our templated +// code can print it to spdlog. +namespace fmt { +template <> +struct formatter : drake::ostream_formatter {}; +} // namespace fmt + +// Instantiate templates. +template class drake::solvers::MobyLCPSolver; +template class drake::solvers::MobyLCPSolver; diff --git a/solvers/moby_lcp_solver.h b/solvers/moby_lcp_solver.h index 75ca065ec811..45bb14205c95 100644 --- a/solvers/moby_lcp_solver.h +++ b/solvers/moby_lcp_solver.h @@ -11,6 +11,7 @@ #include #include "drake/common/drake_copyable.h" +#include "drake/common/drake_deprecated.h" #include "drake/solvers/mathematical_program.h" #include "drake/solvers/solver_base.h" @@ -74,7 +75,8 @@ class MobyLCPSolver final : public SolverBase { MobyLCPSolver(); ~MobyLCPSolver() final; - void SetLoggingEnabled(bool enabled); + DRAKE_DEPRECATED("2025-05-01", "Use drake/common/text_logging.h instead.") + void SetLoggingEnabled(bool) {} /// Calculates the zero tolerance that the solver would compute if the user /// does not specify a tolerance. @@ -292,12 +294,6 @@ class MobyLCPSolver final : public SolverBase { void FinishLemkeSolution(const MatrixType& M, const VectorX& q, const VectorX& x, VectorX* z) const; - // TODO(sammy-tri) replace this with a proper logging hookup - std::ostream& Log() const; - - bool log_enabled_{false}; - mutable std::ofstream null_stream_; - // Records the number of pivoting operations used during the last solve. mutable unsigned pivots_{0}; diff --git a/solvers/test/moby_lcp_solver_test.cc b/solvers/test/moby_lcp_solver_test.cc index b411894d1545..e4e02607149c 100644 --- a/solvers/test/moby_lcp_solver_test.cc +++ b/solvers/test/moby_lcp_solver_test.cc @@ -13,7 +13,6 @@ namespace solvers { namespace { const double epsilon = 1e-6; -const bool verbose = false; /// Run all non-regularized solvers. If @p expected_z is an empty /// vector, outputs will only be compared against each other. @@ -21,7 +20,6 @@ template void RunBasicLcp(const Eigen::MatrixBase& M, const Eigen::VectorXd& q, const Eigen::VectorXd& expected_z_in, bool expect_fast_pass) { MobyLCPSolver l; - l.SetLoggingEnabled(verbose); Eigen::VectorXd expected_z = expected_z_in; @@ -60,7 +58,6 @@ void RunRegularizedLcp(const Eigen::MatrixBase& M, const Eigen::VectorXd& expected_z_in, bool expect_fast_pass) { MobyLCPSolver l; - l.SetLoggingEnabled(verbose); Eigen::VectorXd expected_z = expected_z_in; @@ -145,7 +142,6 @@ GTEST_TEST(testMobyLCP, testAutoDiffTrivial) { // Attempt to compute the solution using both "fast" and Lemke algorithms. MobyLCPSolver l; - l.SetLoggingEnabled(verbose); bool result = l.SolveLcpFast(M, q, &fast_z); EXPECT_TRUE(result); result = l.SolveLcpLemke(M, q, &lemke_z); @@ -266,7 +262,6 @@ GTEST_TEST(testMobyLCP, testProblem4) { z << 1. / 90., 2. / 45., 1. / 90., 2. / 45.; MobyLCPSolver l; - l.SetLoggingEnabled(verbose); Eigen::VectorXd fast_z; bool result = l.SolveLcpFast(M, q, &fast_z); @@ -326,7 +321,6 @@ GTEST_TEST(testMobyLCP, testEmpty) { Eigen::VectorXd empty_q(0); Eigen::VectorXd z; MobyLCPSolver l; - l.SetLoggingEnabled(verbose); bool result = l.SolveLcpFast(empty_M, empty_q, &z); EXPECT_TRUE(result); @@ -356,7 +350,6 @@ GTEST_TEST(testMobyLCP, testFailure) { neg_q[0] = -1; Eigen::VectorXd z; MobyLCPSolver l; - l.SetLoggingEnabled(verbose); bool result = l.SolveLcpFast(neg_M, neg_q, &z); EXPECT_FALSE(result);