From 7e113c0a8e94ebc17264b308a5898d523ec275bb Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 31 Mar 2024 15:23:25 -0700 Subject: [PATCH] w --- dart/common/ThreadPool.cpp | 43 ++ dart/common/ThreadPool.hpp | 113 ++++ dart/constraint/BoxedLcpConstraintSolver.cpp | 533 +++++++++++++------ dart/constraint/BoxedLcpConstraintSolver.hpp | 162 ++++-- dart/constraint/ConstraintSolver.cpp | 2 + dart/constraint/ConstraintSolver.hpp | 10 +- examples/boxes/main.cpp | 2 +- pixi.toml | 9 +- tests/benchmark/integration/bm_boxes.cpp | 2 +- 9 files changed, 648 insertions(+), 228 deletions(-) create mode 100644 dart/common/ThreadPool.cpp create mode 100644 dart/common/ThreadPool.hpp diff --git a/dart/common/ThreadPool.cpp b/dart/common/ThreadPool.cpp new file mode 100644 index 0000000000000..fb1a5ea0d88a9 --- /dev/null +++ b/dart/common/ThreadPool.cpp @@ -0,0 +1,43 @@ +/* + * Copyright (c) 2011-2024, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/main/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/common/ThreadPool.hpp" + +#include "dart/common/Console.hpp" +#include "dart/common/Logging.hpp" +#include "dart/common/Macros.hpp" + +namespace dart::common { + +//============================================================================== + +} // namespace dart::common diff --git a/dart/common/ThreadPool.hpp b/dart/common/ThreadPool.hpp new file mode 100644 index 0000000000000..5132a760a465a --- /dev/null +++ b/dart/common/ThreadPool.hpp @@ -0,0 +1,113 @@ +/* + * Copyright (c) 2011-2024, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/main/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace dart::common { + +class ThreadPool +{ +public: + ThreadPool(size_t threads) : stop(false) + { + for (size_t i = 0; i < threads; ++i) + workers.emplace_back([this] { + while (true) { + std::function task; + + { + std::unique_lock lock(this->queueMutex); + this->condition.wait( + lock, [this] { return this->stop || !this->tasks.empty(); }); + if (this->stop && this->tasks.empty()) + return; + task = std::move(this->tasks.front()); + this->tasks.pop(); + } + + task(); + } + }); + } + + template + auto enqueue(F&& f, Args&&... args) + -> std::future::type> + { + using return_type = typename std::result_of::type; + + auto task = std::make_shared>( + std::bind(std::forward(f), std::forward(args)...)); + + std::future res = task->get_future(); + { + std::unique_lock lock(queueMutex); + + // don't allow enqueueing after stopping the pool + if (stop) + throw std::runtime_error("enqueue on stopped ThreadPool"); + + tasks.emplace([task]() { (*task)(); }); + } + condition.notify_one(); + return res; + } + + ~ThreadPool() + { + { + std::unique_lock lock(queueMutex); + stop = true; + } + condition.notify_all(); + for (std::thread& worker : workers) + worker.join(); + } + +private: + std::vector workers; + std::queue> tasks; + + std::mutex queueMutex; + std::condition_variable condition; + bool stop; +}; + +} // namespace dart::common diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index c455b3416af62..7a4a8c7595e9f 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.cpp +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -32,24 +32,191 @@ #include "dart/constraint/BoxedLcpConstraintSolver.hpp" -#include -#if DART_BUILD_MODE_DEBUG - #include - #include -#endif - -#include "dart/common/Console.hpp" +#include "dart/common/Logging.hpp" #include "dart/common/Profile.hpp" +#include "dart/common/ThreadPool.hpp" #include "dart/constraint/ConstraintBase.hpp" #include "dart/constraint/DantzigBoxedLcpSolver.hpp" #include "dart/constraint/PgsBoxedLcpSolver.hpp" #include "dart/external/odelcpsolver/lcp.h" #include "dart/lcpsolver/Lemke.hpp" +#if DART_BUILD_MODE_DEBUG + #include + #include +#endif +#include +#include +#include + +#include + namespace dart { namespace constraint { +namespace { + +[[nodiscard]] BoxedLcpSolverPtr createBoxedLcpSolver( + const BoxedLcpSolverType type) +{ + switch (type) { + case BoxedLcpSolverType::Dantzig: { + return std::make_shared(); + } + case BoxedLcpSolverType::Pgs: { + return std::make_shared(); + } + default: { + DART_WARN("Unknown BoxedLcpSolverType. Using Dantzig solver instead."); + return std::make_shared(); + } + } +} + +#if DART_BUILD_MODE_DEBUG +bool isSymmetric(std::size_t n, double* A) +{ + std::size_t nSkip = dPAD(n); + for (std::size_t i = 0; i < n; ++i) { + for (std::size_t j = 0; j < n; ++j) { + if (std::abs(A[nSkip * i + j] - A[nSkip * j + i]) > 1e-6) { + std::cout << "A: " << std::endl; + for (std::size_t k = 0; k < n; ++k) { + for (std::size_t l = 0; l < nSkip; ++l) { + std::cout << std::setprecision(4) << A[k * nSkip + l] << " "; + } + std::cout << std::endl; + } + + std::cout << "A(" << i << ", " << j << "): " << A[nSkip * i + j] + << std::endl; + std::cout << "A(" << j << ", " << i << "): " << A[nSkip * j + i] + << std::endl; + return false; + } + } + } + + return true; +} + //============================================================================== +bool isSymmetric(std::size_t n, double* A, std::size_t begin, std::size_t end) +{ + std::size_t nSkip = dPAD(n); + for (std::size_t i = begin; i <= end; ++i) { + for (std::size_t j = begin; j <= end; ++j) { + if (std::abs(A[nSkip * i + j] - A[nSkip * j + i]) > 1e-6) { + std::cout << "A: " << std::endl; + for (std::size_t k = 0; k < n; ++k) { + for (std::size_t l = 0; l < nSkip; ++l) { + std::cout << std::setprecision(4) << A[k * nSkip + l] << " "; + } + std::cout << std::endl; + } + + std::cout << "A(" << i << ", " << j << "): " << A[nSkip * i + j] + << std::endl; + std::cout << "A(" << j << ", " << i << "): " << A[nSkip * j + i] + << std::endl; + return false; + } + } + } + + return true; +} + +//============================================================================== +void print( + std::size_t n, + double* A, + double* x, + double* /*lo*/, + double* /*hi*/, + double* b, + double* w, + int* findex) +{ + std::size_t nSkip = dPAD(n); + std::cout << "A: " << std::endl; + for (std::size_t i = 0; i < n; ++i) { + for (std::size_t j = 0; j < nSkip; ++j) { + std::cout << std::setprecision(4) << A[i * nSkip + j] << " "; + } + std::cout << std::endl; + } + + std::cout << "b: "; + for (std::size_t i = 0; i < n; ++i) { + std::cout << std::setprecision(4) << b[i] << " "; + } + std::cout << std::endl; + + std::cout << "w: "; + for (std::size_t i = 0; i < n; ++i) { + std::cout << w[i] << " "; + } + std::cout << std::endl; + + std::cout << "x: "; + for (std::size_t i = 0; i < n; ++i) { + std::cout << x[i] << " "; + } + std::cout << std::endl; + + // std::cout << "lb: "; + // for (int i = 0; i < dim; ++i) + // { + // std::cout << lb[i] << " "; + // } + // std::cout << std::endl; + + // std::cout << "ub: "; + // for (int i = 0; i < dim; ++i) + // { + // std::cout << ub[i] << " "; + // } + // std::cout << std::endl; + + std::cout << "frictionIndex: "; + for (std::size_t i = 0; i < n; ++i) { + std::cout << findex[i] << " "; + } + std::cout << std::endl; + + double* Ax = new double[n]; + + for (std::size_t i = 0; i < n; ++i) { + Ax[i] = 0.0; + } + + for (std::size_t i = 0; i < n; ++i) { + for (std::size_t j = 0; j < n; ++j) { + Ax[i] += A[i * nSkip + j] * x[j]; + } + } + + std::cout << "Ax : "; + for (std::size_t i = 0; i < n; ++i) { + std::cout << Ax[i] << " "; + } + std::cout << std::endl; + + std::cout << "b + w: "; + for (std::size_t i = 0; i < n; ++i) { + std::cout << b[i] + w[i] << " "; + } + std::cout << std::endl; + + delete[] Ax; +} +#endif + +} // namespace + +//============================================================================== +DART_SUPPRESS_DEPRECATED_BEGIN BoxedLcpConstraintSolver::BoxedLcpConstraintSolver( double timeStep, BoxedLcpSolverPtr boxedLcpSolver, @@ -59,15 +226,17 @@ BoxedLcpConstraintSolver::BoxedLcpConstraintSolver( { setTimeStep(timeStep); } +DART_SUPPRESS_DEPRECATED_END //============================================================================== -BoxedLcpConstraintSolver::BoxedLcpConstraintSolver() - : BoxedLcpConstraintSolver(std::make_shared()) +BoxedLcpConstraintSolver::BoxedLcpConstraintSolver(const Config& config) + : ConstraintSolver(), mConfig(config), mThreadPool(4) { - // Do nothing + // Empty } //============================================================================== +DART_SUPPRESS_DEPRECATED_BEGIN BoxedLcpConstraintSolver::BoxedLcpConstraintSolver( BoxedLcpSolverPtr boxedLcpSolver) : BoxedLcpConstraintSolver( @@ -75,37 +244,70 @@ BoxedLcpConstraintSolver::BoxedLcpConstraintSolver( { // Do nothing } +DART_SUPPRESS_DEPRECATED_END //============================================================================== +DART_SUPPRESS_DEPRECATED_BEGIN BoxedLcpConstraintSolver::BoxedLcpConstraintSolver( BoxedLcpSolverPtr boxedLcpSolver, BoxedLcpSolverPtr secondaryBoxedLcpSolver) - : ConstraintSolver() + : ConstraintSolver(), mThreadPool(4) { if (boxedLcpSolver) { setBoxedLcpSolver(std::move(boxedLcpSolver)); } else { - dtwarn << "[BoxedLcpConstraintSolver] Attempting to construct with nullptr " - << "LCP solver, which is not allowed. Using Dantzig solver " - << "instead.\n"; + DART_WARN( + "[BoxedLcpConstraintSolver] Attempting to construct with nullptr LCP " + "solver, which is not allowed. Using Dantzig solver instead."); setBoxedLcpSolver(std::make_shared()); } setSecondaryBoxedLcpSolver(std::move(secondaryBoxedLcpSolver)); } +DART_SUPPRESS_DEPRECATED_END + +//============================================================================== +void BoxedLcpConstraintSolver::setPrimaryBoxedLcpSolverType( + BoxedLcpSolverType type) +{ + mConfig.primaryBoxedLcpSolver = type; +} + +//============================================================================== +BoxedLcpSolverType BoxedLcpConstraintSolver::getPrimaryBoxedLcpSolverType() + const +{ + return mConfig.primaryBoxedLcpSolver; +} + +//============================================================================== +void BoxedLcpConstraintSolver::setSecondaryBoxedLcpSolverType( + BoxedLcpSolverType type) +{ + mConfig.secondaryBoxedLcpSolver = type; +} + +//============================================================================== +BoxedLcpSolverType BoxedLcpConstraintSolver::getSecondaryBoxedLcpSolverType() + const +{ + return mConfig.secondaryBoxedLcpSolver; +} //============================================================================== void BoxedLcpConstraintSolver::setBoxedLcpSolver(BoxedLcpSolverPtr lcpSolver) { if (!lcpSolver) { - dtwarn << "[BoxedLcpConstraintSolver::setBoxedLcpSolver] " - << "nullptr for boxed LCP solver is not allowed.\n"; + DART_WARN( + "[BoxedLcpConstraintSolver::setBoxedLcpSolver] nullptr for boxed LCP " + "solver is not allowed."); return; } if (lcpSolver == mSecondaryBoxedLcpSolver) { - dtwarn << "[BoxedLcpConstraintSolver::setBoxedLcpSolver] Attempting to set " - << "a primary LCP solver that is the same with the secondary LCP " - << "solver, which is discouraged. Ignoring this request.\n"; + DART_WARN( + "[BoxedLcpConstraintSolver::setBoxedLcpSolver] Attempting to set a " + "primary LCP solver that is the same with the secondary LCP solver, " + "which is discouraged. Ignoring this request."); } mBoxedLcpSolver = std::move(lcpSolver); @@ -122,10 +324,11 @@ void BoxedLcpConstraintSolver::setSecondaryBoxedLcpSolver( BoxedLcpSolverPtr lcpSolver) { if (lcpSolver == mBoxedLcpSolver) { - dtwarn << "[BoxedLcpConstraintSolver::setBoxedLcpSolver] Attempting to set " - << "the secondary LCP solver that is identical to the primary LCP " - << "solver, which is redundant. Please use different solvers or set " - << "the secondary LCP solver to nullptr.\n"; + DART_WARN( + "[BoxedLcpConstraintSolver::setSecondaryBoxedLcpSolver] Attempting " + "to set the secondary LCP solver that is identical to the primary " + "LCP solver, which is redundant. Please use different solvers or " + "set the secondary LCP solver to nullptr."); } mSecondaryBoxedLcpSolver = std::move(lcpSolver); @@ -139,54 +342,70 @@ ConstBoxedLcpSolverPtr BoxedLcpConstraintSolver::getSecondaryBoxedLcpSolver() } //============================================================================== -void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group) +void solveBoxedLcp(BoxedLcp& lcp, ConstrainedGroup& group) { DART_PROFILE_SCOPED; + (void)lcp; + (void)group; // Build LCP terms by aggregating them from constraints const std::size_t numConstraints = group.getNumConstraints(); const std::size_t n = group.getTotalDimension(); // If there is no constraint, then just return. - if (0u == n) + if (0u == n) { return; + } + + auto& mA = lcp.A; + auto& mABackup = lcp.ABackup; + auto& mX = lcp.x; + auto& mXBackup = lcp.xBackup; + auto& mB = lcp.b; + auto& mBBackup = lcp.bBackup; + // auto& mW = lcp.w; + auto& mLo = lcp.lo; + auto& mLoBackup = lcp.loBackup; + auto& mHi = lcp.hi; + auto& mHiBackup = lcp.hiBackup; + auto& mFIndex = lcp.fIndex; + auto& mFIndexBackup = lcp.fIndexBackup; + auto& mOffset = lcp.offset; + auto& mBoxedLcpSolver = lcp.boxedLcpSolver; + auto& mSecondaryBoxedLcpSolver = lcp.secondaryBoxedLcpSolver; const int nSkip = dPAD(n); -#if DART_BUILD_MODE_RELEASE - mA.resize(n, nSkip); -#else // debug - mA.setZero(n, nSkip); -#endif - mX.resize(n); - mB.resize(n); - mW.setZero(n); // set w to 0 - mLo.resize(n); - mHi.resize(n); - mFIndex.setConstant(n, -1); // set findex to -1 + lcp.A.resize(n, nSkip); + lcp.x.resize(n); + lcp.b.resize(n); + lcp.w.setZero(n); + lcp.lo.resize(n); + lcp.hi.resize(n); + lcp.fIndex.setConstant(n, -1); // Compute offset indices - mOffset.resize(numConstraints); - mOffset[0] = 0; + lcp.offset.resize(numConstraints); + lcp.offset[0] = 0; for (std::size_t i = 1; i < numConstraints; ++i) { - const ConstraintBasePtr& constraint = group.getConstraint(i - 1); + const auto constraint = group.getConstraint(i - 1); assert(constraint->getDimension() > 0); - mOffset[i] = mOffset[i - 1] + constraint->getDimension(); + lcp.offset[i] = lcp.offset[i - 1] + constraint->getDimension(); } // For each constraint { DART_PROFILE_SCOPED_N("Construct LCP"); ConstraintInfo constInfo; - constInfo.invTimeStep = 1.0 / mTimeStep; + constInfo.invTimeStep = 1.0 / lcp.timeStep; for (std::size_t i = 0; i < numConstraints; ++i) { - const ConstraintBasePtr& constraint = group.getConstraint(i); + const auto constraint = group.getConstraint(i); - constInfo.x = mX.data() + mOffset[i]; - constInfo.lo = mLo.data() + mOffset[i]; - constInfo.hi = mHi.data() + mOffset[i]; - constInfo.b = mB.data() + mOffset[i]; - constInfo.findex = mFIndex.data() + mOffset[i]; - constInfo.w = mW.data() + mOffset[i]; + constInfo.x = lcp.x.data() + lcp.offset[i]; + constInfo.lo = lcp.lo.data() + lcp.offset[i]; + constInfo.hi = lcp.hi.data() + lcp.offset[i]; + constInfo.b = lcp.b.data() + lcp.offset[i]; + constInfo.findex = lcp.fIndex.data() + lcp.offset[i]; + constInfo.w = lcp.w.data() + lcp.offset[i]; // Fill vectors: lo, hi, b, w { @@ -200,8 +419,9 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group) constraint->excite(); for (std::size_t j = 0; j < constraint->getDimension(); ++j) { // Adjust findex for global index - if (mFIndex[mOffset[i] + j] >= 0) - mFIndex[mOffset[i] + j] += mOffset[i]; + if (lcp.fIndex[lcp.offset[i] + j] >= 0) { + lcp.fIndex[lcp.offset[i] + j] += lcp.offset[i]; + } // Apply impulse for impulse test { @@ -212,10 +432,10 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group) // Fill upper triangle blocks of A matrix { DART_PROFILE_SCOPED_N("Fill upper triangle of A"); - int index = nSkip * (mOffset[i] + j) + mOffset[i]; + int index = nSkip * (lcp.offset[i] + j) + lcp.offset[i]; constraint->getVelocityChange(mA.data() + index, true); for (std::size_t k = i + 1; k < numConstraints; ++k) { - index = nSkip * (mOffset[i] + j) + mOffset[k]; + index = nSkip * (lcp.offset[i] + j) + lcp.offset[k]; group.getConstraint(k)->getVelocityChange( mA.data() + index, false); } @@ -271,8 +491,9 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group) // Sanity check. LCP solvers should not report success with nan values, but // it could happen. So we set the success to false for nan values. - if (success && mX.hasNaN()) + if (success && mX.hasNaN()) { success = false; + } if (!success && mSecondaryBoxedLcpSolver) { DART_PROFILE_SCOPED_N("Secondary LCP"); @@ -315,146 +536,104 @@ void BoxedLcpConstraintSolver::solveConstrainedGroup(ConstrainedGroup& group) } //============================================================================== -#if DART_BUILD_MODE_DEBUG -bool BoxedLcpConstraintSolver::isSymmetric(std::size_t n, double* A) +void BoxedLcpConstraintSolver::solveConstrainedGroups() { - std::size_t nSkip = dPAD(n); - for (std::size_t i = 0; i < n; ++i) { - for (std::size_t j = 0; j < n; ++j) { - if (std::abs(A[nSkip * i + j] - A[nSkip * j + i]) > 1e-6) { - std::cout << "A: " << std::endl; - for (std::size_t k = 0; k < n; ++k) { - for (std::size_t l = 0; l < nSkip; ++l) { - std::cout << std::setprecision(4) << A[k * nSkip + l] << " "; - } - std::cout << std::endl; - } - - std::cout << "A(" << i << ", " << j << "): " << A[nSkip * i + j] - << std::endl; - std::cout << "A(" << j << ", " << i << "): " << A[nSkip * j + i] - << std::endl; - return false; - } - } - } - - return true; -} + DART_PROFILE_SCOPED; -//============================================================================== -bool BoxedLcpConstraintSolver::isSymmetric( - std::size_t n, double* A, std::size_t begin, std::size_t end) -{ - std::size_t nSkip = dPAD(n); - for (std::size_t i = begin; i <= end; ++i) { - for (std::size_t j = begin; j <= end; ++j) { - if (std::abs(A[nSkip * i + j] - A[nSkip * j + i]) > 1e-6) { - std::cout << "A: " << std::endl; - for (std::size_t k = 0; k < n; ++k) { - for (std::size_t l = 0; l < nSkip; ++l) { - std::cout << std::setprecision(4) << A[k * nSkip + l] << " "; - } - std::cout << std::endl; - } + const auto numGroups = mConstrainedGroups.size(); - std::cout << "A(" << i << ", " << j << "): " << A[nSkip * i + j] - << std::endl; - std::cout << "A(" << j << ", " << i << "): " << A[nSkip * j + i] - << std::endl; - return false; + // Prepare problems + { + DART_PROFILE_SCOPED_N("Prepare problems"); + mProblems.resize(numGroups); + for (auto& prob : mProblems) { + if (!prob.boxedLcpSolver) { + prob.boxedLcpSolver + = createBoxedLcpSolver(mConfig.primaryBoxedLcpSolver); } - } - } - return true; -} + if (!prob.secondaryBoxedLcpSolver) { + prob.secondaryBoxedLcpSolver + = createBoxedLcpSolver(mConfig.secondaryBoxedLcpSolver); + } -//============================================================================== -void BoxedLcpConstraintSolver::print( - std::size_t n, - double* A, - double* x, - double* /*lo*/, - double* /*hi*/, - double* b, - double* w, - int* findex) -{ - std::size_t nSkip = dPAD(n); - std::cout << "A: " << std::endl; - for (std::size_t i = 0; i < n; ++i) { - for (std::size_t j = 0; j < nSkip; ++j) { - std::cout << std::setprecision(4) << A[i * nSkip + j] << " "; + prob.timeStep = mTimeStep; } - std::cout << std::endl; } - std::cout << "b: "; - for (std::size_t i = 0; i < n; ++i) { - std::cout << std::setprecision(4) << b[i] << " "; - } - std::cout << std::endl; - - std::cout << "w: "; - for (std::size_t i = 0; i < n; ++i) { - std::cout << w[i] << " "; - } - std::cout << std::endl; + // Solve problems + // for (auto i = 0u; i < numGroups; ++i) { + // solveBoxedLcp(mProblems[i], mConstrainedGroups[i]); + // } + + // Solve problems in multi-thread using std::thread + // std::vector threads; + // threads.reserve(numGroups); + + // for (auto i = 0u; i < numGroups; ++i) { + // // Capture `i` by value to ensure each thread uses the correct index + // threads.emplace_back( + // [this, i]() { solveBoxedLcp(mProblems[i], mConstrainedGroups[i]); }); + // } + + // // Join threads + // for (auto& thread : threads) { + // if (thread.joinable()) { + // thread.join(); + // } + // } + + // { + // DART_PROFILE_SCOPED; + + // // Determine the number of available cores + // // const unsigned int numThreads = std::thread::hardware_concurrency(); + // const unsigned int numThreads = 16; + + // // Use futures and async to manage the tasks + // std::vector> futures; + + // for (auto i = 0u; i < numGroups; ++i) { + // // Check if we've reached the concurrency limit + // if (futures.size() >= numThreads) { + // // Wait for one of the futures to finish + // for (auto& fut : futures) { + // if (fut.wait_for(std::chrono::seconds(0)) + // == std::future_status::ready) { + // fut.get(); // Clear the completed future + // break; // Break the loop to add a new task + // } + // } + // } + + // // Launch a new task + // futures.push_back(std::async(std::launch::async, [this, i]() { + // solveBoxedLcp(mProblems[i], mConstrainedGroups[i]); + // })); + // } + + // // Ensure all tasks are completed + // for (auto& fut : futures) { + // fut.get(); + // } + // } - std::cout << "x: "; - for (std::size_t i = 0; i < n; ++i) { - std::cout << x[i] << " "; - } - std::cout << std::endl; - - // std::cout << "lb: "; - // for (int i = 0; i < dim; ++i) - // { - // std::cout << lb[i] << " "; - // } - // std::cout << std::endl; - - // std::cout << "ub: "; - // for (int i = 0; i < dim; ++i) - // { - // std::cout << ub[i] << " "; - // } - // std::cout << std::endl; - - std::cout << "frictionIndex: "; - for (std::size_t i = 0; i < n; ++i) { - std::cout << findex[i] << " "; - } - std::cout << std::endl; - - double* Ax = new double[n]; + { + DART_PROFILE_SCOPED_N("Solve problems"); - for (std::size_t i = 0; i < n; ++i) { - Ax[i] = 0.0; - } + std::vector> results; - for (std::size_t i = 0; i < n; ++i) { - for (std::size_t j = 0; j < n; ++j) { - Ax[i] += A[i * nSkip + j] * x[j]; + for (auto i = 0u; i < numGroups; ++i) { + results.emplace_back(mThreadPool.enqueue( + [this, i]() { solveBoxedLcp(mProblems[i], mConstrainedGroups[i]); })); } - } - std::cout << "Ax : "; - for (std::size_t i = 0; i < n; ++i) { - std::cout << Ax[i] << " "; - } - std::cout << std::endl; - - std::cout << "b + w: "; - for (std::size_t i = 0; i < n; ++i) { - std::cout << b[i] + w[i] << " "; + // Wait for all tasks to complete + for (auto&& result : results) { + result.get(); + } } - std::cout << std::endl; - - delete[] Ax; } -#endif } // namespace constraint } // namespace dart diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp index 3220c1876aaad..1661bd59a1241 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -35,13 +35,84 @@ #include #include +#include namespace dart { namespace constraint { +struct BoxedLcp +{ + using RowMajorMatrixXd + = Eigen::Matrix; + + /// Cache data for boxed LCP formulation + RowMajorMatrixXd A; + + /// Cache data for boxed LCP formulation + RowMajorMatrixXd ABackup; + + /// Cache data for boxed LCP formulation + Eigen::VectorXd x; + + /// Cache data for boxed LCP formulation + Eigen::VectorXd xBackup; + + /// Cache data for boxed LCP formulation + Eigen::VectorXd b; + + /// Cache data for boxed LCP formulation + Eigen::VectorXd bBackup; + + /// Cache data for boxed LCP formulation + Eigen::VectorXd w; + + /// Cache data for boxed LCP formulation + Eigen::VectorXd lo; + + /// Cache data for boxed LCP formulation + Eigen::VectorXd loBackup; + + /// Cache data for boxed LCP formulation + Eigen::VectorXd hi; + + /// Cache data for boxed LCP formulation + Eigen::VectorXd hiBackup; + + /// Cache data for boxed LCP formulation + Eigen::VectorXi fIndex; + + /// Cache data for boxed LCP formulation + Eigen::VectorXi fIndexBackup; + + /// Cache data for boxed LCP formulation + Eigen::VectorXi offset; + + /// Boxed LCP solver + BoxedLcpSolverPtr boxedLcpSolver; + + /// Boxed LCP solver to be used when the primary solver failed + BoxedLcpSolverPtr secondaryBoxedLcpSolver; + + double timeStep{-1}; +}; + +enum class BoxedLcpSolverType +{ + Dantzig, ///< The Dantzig solver + Pgs, ///< The projected Gauss-Seidel solver +}; + +struct BoxedLcpConstraintSolverConfig +{ + BoxedLcpSolverType primaryBoxedLcpSolver = BoxedLcpSolverType::Dantzig; + BoxedLcpSolverType secondaryBoxedLcpSolver = BoxedLcpSolverType::Pgs; +}; + class BoxedLcpConstraintSolver : public ConstraintSolver { public: + using Config = BoxedLcpConstraintSolverConfig; + /// Constructor /// /// \param[in] timeStep Simulation time step @@ -51,25 +122,22 @@ class BoxedLcpConstraintSolver : public ConstraintSolver /// nullptr is passed, PGS solver will be used. This is to make the default /// solver setting to be Dantzig + PGS. In order to disable use of secondary /// solver, call setSecondaryBoxedLcpSolver(nullptr) explicitly. - /// - /// \deprecated Deprecated in DART 6.8. Please use other constructors that - /// doesn't take timespte. Timestep should be set by the owner of this solver - /// such as dart::simulation::World when the solver added. - DART_DEPRECATED(6.8) - BoxedLcpConstraintSolver( - double timeStep, - BoxedLcpSolverPtr boxedLcpSolver = nullptr, - BoxedLcpSolverPtr secondaryBoxedLcpSolver = nullptr); - - /// Constructos with default primary and secondary LCP solvers, which are - /// Dantzig and PGS, respectively. - BoxedLcpConstraintSolver(); + [[deprecated( + "Since DART 6.14. Please use other constructor that takes config " + "struct instead.")]] BoxedLcpConstraintSolver(double timeStep, BoxedLcpSolverPtr boxedLcpSolver = nullptr, BoxedLcpSolverPtr secondaryBoxedLcpSolver = nullptr); + + /// Constructs with specific primary and secondary LCP solvers, which are + /// specified by the config struct. + explicit BoxedLcpConstraintSolver(const Config& config = Config()); /// Constructors with specific primary LCP solver. /// /// \param[in] boxedLcpSolver The primary boxed LCP solver. When nullptr is /// passed, which is discouraged, Dantzig solver will be used. - BoxedLcpConstraintSolver(BoxedLcpSolverPtr boxedLcpSolver); + [[deprecated( + "Since DART 6.14. Please use other constructor that takes config " + "struct instead.")]] BoxedLcpConstraintSolver(BoxedLcpSolverPtr + boxedLcpSolver); /// Constructs with specific primary and secondary LCP solvers. /// @@ -77,29 +145,54 @@ class BoxedLcpConstraintSolver : public ConstraintSolver /// passed, which is discouraged, Dantzig solver will be used. /// \param[in] secondaryBoxedLcpSolver The secondary boxed-LCP solver. Pass /// nullptr to disable using secondary LCP solver. - BoxedLcpConstraintSolver( - BoxedLcpSolverPtr boxedLcpSolver, - BoxedLcpSolverPtr secondaryBoxedLcpSolver); + [[deprecated( + "Since DART 6.14. Please use other constructor that takes config " + "struct instead.")]] BoxedLcpConstraintSolver(BoxedLcpSolverPtr boxedLcpSolver, BoxedLcpSolverPtr secondaryBoxedLcpSolver); + + /// Sets the primary boxed LCP solver type + void setPrimaryBoxedLcpSolverType(BoxedLcpSolverType type); + + /// Returns the primary boxed LCP solver type + [[nodiscard]] BoxedLcpSolverType getPrimaryBoxedLcpSolverType() const; + + /// Sets the secondary boxed LCP solver type + void setSecondaryBoxedLcpSolverType(BoxedLcpSolverType type); + + /// Returns the secondary boxed LCP solver type + [[nodiscard]] BoxedLcpSolverType getSecondaryBoxedLcpSolverType() const; /// Sets boxed LCP (BLCP) solver /// /// \param[in] lcpSolver The primary boxed LCP solver. When nullptr is /// passed, Dantzig solver will be used. - void setBoxedLcpSolver(BoxedLcpSolverPtr lcpSolver); + [[deprecated( + "Since DART 6.14. Please use setPrimaryBoxedLcpSolverType() " + "instead.")]] void + setBoxedLcpSolver(BoxedLcpSolverPtr lcpSolver); /// Returns boxed LCP (BLCP) solver - ConstBoxedLcpSolverPtr getBoxedLcpSolver() const; + [[deprecated( + "Since DART 6.14. Please use getPrimaryBoxedLcpSolverType() " + "instead.")]] ConstBoxedLcpSolverPtr + getBoxedLcpSolver() const; /// Sets boxed LCP (BLCP) solver that is used when the primary solver failed - void setSecondaryBoxedLcpSolver(BoxedLcpSolverPtr lcpSolver); + [[deprecated( + "Since DART 6.14. Please use setSecondaryBoxedLcpSolverType() " + "instead.")]] void + setSecondaryBoxedLcpSolver(BoxedLcpSolverPtr lcpSolver); /// Returns boxed LCP (BLCP) solver that is used when the primary solver /// failed - ConstBoxedLcpSolverPtr getSecondaryBoxedLcpSolver() const; + [[deprecated( + "Since DART 6.14. Please use getSecondaryBoxedLcpSolverType() " + "instead.")]] ConstBoxedLcpSolverPtr + getSecondaryBoxedLcpSolver() const; protected: - // Documentation inherited. - void solveConstrainedGroup(ConstrainedGroup& group) override; + void solveConstrainedGroups() override; + + Config mConfig; /// Boxed LCP solver BoxedLcpSolverPtr mBoxedLcpSolver; @@ -154,26 +247,9 @@ class BoxedLcpConstraintSolver : public ConstraintSolver /// Cache data for boxed LCP formulation Eigen::VectorXi mOffset; -#if DART_BUILD_MODE_DEBUG -private: - /// Return true if the matrix is symmetric - bool isSymmetric(std::size_t n, double* A); - - /// Return true if the diagonal block of matrix is symmetric - bool isSymmetric( - std::size_t n, double* A, std::size_t begin, std::size_t end); - - /// Print debug information - void print( - std::size_t n, - double* A, - double* x, - double* lo, - double* hi, - double* b, - double* w, - int* findex); -#endif + std::vector mProblems; + + common::ThreadPool mThreadPool; }; } // namespace constraint diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 7f3331c535412..c36c6b123d134 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -697,7 +697,9 @@ void ConstraintSolver::solveConstrainedGroups() DART_PROFILE_SCOPED; for (auto& constraintGroup : mConstrainedGroups) { + DART_SUPPRESS_DEPRECATED_BEGIN solveConstrainedGroup(constraintGroup); + DART_SUPPRESS_DEPRECATED_END } } diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp index 4935eb5ffda98..d11ab755af6fc 100644 --- a/dart/constraint/ConstraintSolver.hpp +++ b/dart/constraint/ConstraintSolver.hpp @@ -235,8 +235,12 @@ class ConstraintSolver bool removeContactSurfaceHandler(const ContactSurfaceHandlerPtr& handler); protected: - // TODO(JS): Docstring - virtual void solveConstrainedGroup(ConstrainedGroup& group) = 0; + [[deprecated( + "Since DART 6.14. Use solveConstrainedGroups() instead.")]] virtual void + solveConstrainedGroup(ConstrainedGroup& group) + { + (void)group; + } /// Checks if the skeleton is contained in this solver /// @@ -263,7 +267,7 @@ class ConstraintSolver void buildConstrainedGroups(); /// Solve constrained groups - void solveConstrainedGroups(); + virtual void solveConstrainedGroups(); /// Return true if at least one of colliding body is soft body bool isSoftContact(const collision::Contact& contact) const; diff --git a/examples/boxes/main.cpp b/examples/boxes/main.cpp index 13c8b6bb02931..641dca38a5c97 100644 --- a/examples/boxes/main.cpp +++ b/examples/boxes/main.cpp @@ -88,7 +88,7 @@ int main() for (auto k = 0; k < dim; ++k) { auto x = i - dim / 2; auto y = j - dim / 2; - auto z = k + 5; + auto z = k + 1; auto position = Eigen::Vector3d(x, y, z); auto size = Eigen::Vector3d(0.9, 0.9, 0.9); auto color = Eigen::Vector3d( diff --git a/pixi.toml b/pixi.toml index 4be21adb8cd8e..41a3810b910e7 100644 --- a/pixi.toml +++ b/pixi.toml @@ -48,13 +48,16 @@ install-local = { cmd = "cmake --install build --prefix $CONDA_PREFIX", depends_ configure-unix = { cmd = "cmake -G Ninja -S . -B build -DCMAKE_BUILD_TYPE=Release -DDART_VERBOSE=ON -DDART_USE_SYSTEM_IMGUI=ON" } configure-win = { cmd = "cmake -S . -B build -G 'Visual Studio 17 2022' -DDART_VERBOSE=ON -DDART_MSVC_DEFAULT_OPTIONS=ON -DBUILD_SHARED_LIBS=OFF -DDART_USE_SYSTEM_IMGUI=OFF" } -example-hello-world = { cmd = "cmake --build build --target hello_world --parallel && ./build/bin/hello_world", depends_on = [ +ex-hello-world = { cmd = "cmake --build build --target hello_world --parallel && ./build/bin/hello_world", depends_on = [ "configure", ] } -example-atlas-puppet = { cmd = "cmake --build build --target atlas_puppet --parallel && ./build/bin/atlas_puppet", depends_on = [ +ex-atlas-puppet = { cmd = "cmake --build build --target atlas_puppet --parallel && ./build/bin/atlas_puppet", depends_on = [ "configure", ] } -example-atlas-simbicon = { cmd = "cmake --build build --target atlas_simbicon --parallel && ./build/bin/atlas_simbicon", depends_on = [ +ex-atlas-simbicon = { cmd = "cmake --build build --target atlas_simbicon --parallel && ./build/bin/atlas_simbicon", depends_on = [ + "configure", +] } +ex-boxes = { cmd = "cmake --build build --target boxes --parallel && ./build/bin/boxes", depends_on = [ "configure", ] } diff --git a/tests/benchmark/integration/bm_boxes.cpp b/tests/benchmark/integration/bm_boxes.cpp index ca61f00147636..17cfde427add0 100644 --- a/tests/benchmark/integration/bm_boxes.cpp +++ b/tests/benchmark/integration/bm_boxes.cpp @@ -135,4 +135,4 @@ static void BM_RunBoxes(benchmark::State& state) } } -BENCHMARK(BM_RunBoxes)->Arg(2)->Arg(5)->Unit(benchmark::kMillisecond); +BENCHMARK(BM_RunBoxes)->Arg(2)->Arg(4)->Arg(8)->Unit(benchmark::kMillisecond);