diff --git a/CHANGELOG.md b/CHANGELOG.md index cbc5b396dc629..63f8770059505 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,10 @@ ### DART 6.7.0 (201X-XX-XX) +* Dynamics + + * Refactor constraint solver: [#1099](https://github.com/dartsim/dart/pull/1099) + * GUI * Reorganized OpenGL and GLUT files: [#1088](https://github.com/dartsim/dart/pull/1088) diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp new file mode 100644 index 0000000000000..009275b3c316e --- /dev/null +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -0,0 +1,368 @@ +/* + * Copyright (c) 2011-2018, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/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/constraint/BoxedLcpConstraintSolver.hpp" + +#include +#ifndef NDEBUG +#include +#include +#endif + +#include "dart/external/odelcpsolver/lcp.h" + +#include "dart/common/Console.hpp" +#include "dart/constraint/ConstraintBase.hpp" +#include "dart/constraint/ConstrainedGroup.hpp" +#include "dart/constraint/DantzigBoxedLcpSolver.hpp" +#include "dart/lcpsolver/Lemke.hpp" + +namespace dart { +namespace constraint { + +//============================================================================== +BoxedLcpConstraintSolver::BoxedLcpConstraintSolver( + double timeStep, BoxedLcpSolverPtr boxedLcpSolver) + : ConstraintSolver(timeStep), mBoxedLcpSolver(std::move(boxedLcpSolver)) +{ + if (!mBoxedLcpSolver) + mBoxedLcpSolver = std::make_shared(); +} + +//============================================================================== +void BoxedLcpConstraintSolver::setBoxedLcpSolver( + BoxedLcpSolverPtr lcpSolver) +{ + if (!lcpSolver) + { + dtwarn << "[BoxedLcpConstraintSolver::setBoxedLcpSolver] " + << "nullptr for boxed LCP solver is not allowed."; + return; + } + + mBoxedLcpSolver = std::move(lcpSolver); +} + +//============================================================================== +ConstBoxedLcpSolverPtr BoxedLcpConstraintSolver::getBoxedLcpSolver() const +{ + return mBoxedLcpSolver; +} + +//============================================================================== +void BoxedLcpConstraintSolver::solveConstrainedGroup( + ConstrainedGroup& group) +{ + // Build LCP terms by aggregating them from constraints + std::size_t numConstraints = group.getNumConstraints(); + std::size_t n = group.getTotalDimension(); + + // If there is no constraint, then just return. + if (0u == n) + return; + + int nSkip = dPAD(n); + double* A = new double[n * nSkip]; + double* x = new double[n]; + double* b = new double[n]; + double* w = new double[n]; + double* lo = new double[n]; + double* hi = new double[n]; + int* findex = new int[n]; + + // Set w to 0 and findex to -1 +#ifndef NDEBUG + std::memset(A, 0.0, n * nSkip * sizeof(double)); +#endif + std::memset(w, 0.0, n * sizeof(double)); + std::memset(findex, -1, n * sizeof(int)); + + // Compute offset indices + std::size_t* offset = new std::size_t[n]; + offset[0] = 0; +// std::cout << "offset[" << 0 << "]: " << offset[0] << std::endl; + for (std::size_t i = 1; i < numConstraints; ++i) + { + const ConstraintBasePtr& constraint = group.getConstraint(i - 1); + assert(constraint->getDimension() > 0); + offset[i] = offset[i - 1] + constraint->getDimension(); +// std::cout << "offset[" << i << "]: " << offset[i] << std::endl; + } + + // For each constraint + ConstraintInfo constInfo; + constInfo.invTimeStep = 1.0 / mTimeStep; + for (std::size_t i = 0; i < numConstraints; ++i) + { + const ConstraintBasePtr& constraint = group.getConstraint(i); + + constInfo.x = x + offset[i]; + constInfo.lo = lo + offset[i]; + constInfo.hi = hi + offset[i]; + constInfo.b = b + offset[i]; + constInfo.findex = findex + offset[i]; + constInfo.w = w + offset[i]; + + // Fill vectors: lo, hi, b, w + constraint->getInformation(&constInfo); + + // Fill a matrix by impulse tests: A + constraint->excite(); + for (std::size_t j = 0; j < constraint->getDimension(); ++j) + { + // Adjust findex for global index + if (findex[offset[i] + j] >= 0) + findex[offset[i] + j] += offset[i]; + + // Apply impulse for mipulse test + constraint->applyUnitImpulse(j); + + // Fill upper triangle blocks of A matrix + int index = nSkip * (offset[i] + j) + offset[i]; + constraint->getVelocityChange(A + index, true); + for (std::size_t k = i + 1; k < numConstraints; ++k) + { + index = nSkip * (offset[i] + j) + offset[k]; + group.getConstraint(k)->getVelocityChange(A + index, false); + } + + // Filling symmetric part of A matrix + for (std::size_t k = 0; k < i; ++k) + { + for (std::size_t l = 0; l < group.getConstraint(k)->getDimension(); ++l) + { + int index1 = nSkip * (offset[i] + j) + offset[k] + l; + int index2 = nSkip * (offset[k] + l) + offset[i] + j; + + A[index1] = A[index2]; + } + } + } + + assert(isSymmetric(n, A, offset[i], + offset[i] + constraint->getDimension() - 1)); + + constraint->unexcite(); + } + + assert(isSymmetric(n, A)); + + // Print LCP formulation +// dtdbg << "Before solve:" << std::endl; +// print(n, A, x, lo, hi, b, w, findex); +// std::cout << std::endl; + + // Solve LCP using ODE's Dantzig algorithm + assert(mBoxedLcpSolver); + mBoxedLcpSolver->solve(n, A, x, b, 0, lo, hi, findex); + + // Print LCP formulation +// dtdbg << "After solve:" << std::endl; +// print(n, A, x, lo, hi, b, w, findex); +// std::cout << std::endl; + + // Apply constraint impulses + for (std::size_t i = 0; i < numConstraints; ++i) + { + const ConstraintBasePtr& constraint = group.getConstraint(i); + constraint->applyImpulse(x + offset[i]); + constraint->excite(); + } + + delete[] offset; + + delete[] A; + delete[] x; + delete[] b; + delete[] w; + delete[] lo; + delete[] hi; + delete[] findex; +} + +//============================================================================== +#ifndef NDEBUG +bool BoxedLcpConstraintSolver::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 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; + } + + 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 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] << " "; + } + 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 constraint +} // namespace dart diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp new file mode 100644 index 0000000000000..3ab16e1958e49 --- /dev/null +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -0,0 +1,79 @@ +/* + * Copyright (c) 2011-2018, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/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. + */ + +#ifndef DART_CONSTRAINT_BOXEDLCPCONSTRAINTSOLVER_HPP_ +#define DART_CONSTRAINT_BOXEDLCPCONSTRAINTSOLVER_HPP_ + +#include "dart/constraint/ConstraintSolver.hpp" +#include "dart/constraint/SmartPointer.hpp" + +namespace dart { +namespace constraint { + +class BoxedLcpConstraintSolver : public ConstraintSolver +{ +public: + /// Constructor + BoxedLcpConstraintSolver( + double timeStep, + BoxedLcpSolverPtr boxedLcpSolver = nullptr); + + /// Sets boxed LCP (BLCP) solver + void setBoxedLcpSolver(BoxedLcpSolverPtr lcpSolver); + + /// Returns boxed LCP (BLCP) solver + ConstBoxedLcpSolverPtr getBoxedLcpSolver() const; + +protected: + // Documentation inherited. + void solveConstrainedGroup(ConstrainedGroup& group) override; + + BoxedLcpSolverPtr mBoxedLcpSolver; + +#ifndef NDEBUG +private: + /// Return true if the matrix is symmetric + bool isSymmetric(std::size_t n, double* A); + + /// Return true if the diagonla 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 +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_BOXEDLCPCONSTRAINTSOLVER_HPP_ diff --git a/dart/constraint/BoxedLcpSolver.hpp b/dart/constraint/BoxedLcpSolver.hpp new file mode 100644 index 0000000000000..08594606fef30 --- /dev/null +++ b/dart/constraint/BoxedLcpSolver.hpp @@ -0,0 +1,90 @@ +/* + * Copyright (c) 2011-2018, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/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. + */ + +#ifndef DART_CONSTRAINT_BOXEDLCPSOLVER_HPP_ +#define DART_CONSTRAINT_BOXEDLCPSOLVER_HPP_ + +#include +#include + +namespace dart { +namespace constraint { + +class BoxedLcpSolver +{ +public: + /// Destructor + virtual ~BoxedLcpSolver() = default; + + /// Returns the type + virtual const std::string& getType() const = 0; + + /// Get true if this solver and the template parameter (a solver class) are + /// the same type. This function is a syntactic sugar, which is identical to: + /// \code getType() == ShapeType::getStaticType() \endcode. + /// + /// Example code: + /// \code + /// auto shape = bodyNode->getShapeNode(0)->getShape(); + /// if (shape->is()) + /// std::cout << "The shape type is box!\n"; + /// \endcode + /// + /// \sa getType() + template + bool is() const; + + /// Solves constriant impulses for a constrained group + // Note: The function signature is ODE specific for now. Consider changing + // this to Eigen friendly version once own Dantzig LCP solver is available. + virtual void solve( + int n, + double* A, + double* x, + double* b, + int nub, + double* lo, + double* hi, + int* findex) + = 0; + +#ifndef NDEBUG + virtual bool canSolve(int n, const double* A) = 0; +#endif +}; + +} // namespace constraint +} // namespace dart + +#include "dart/constraint/detail/BoxedLcpSolver-impl.hpp" + +#endif // DART_CONSTRAINT_BOXEDLCPSOLVER_HPP_ diff --git a/dart/constraint/CMakeLists.txt b/dart/constraint/CMakeLists.txt index 9b8924a276b22..8f4f83869dc3a 100644 --- a/dart/constraint/CMakeLists.txt +++ b/dart/constraint/CMakeLists.txt @@ -1,6 +1,8 @@ # Search all header and source files file(GLOB hdrs "*.hpp") file(GLOB srcs "*.cpp") +file(GLOB detail_hdrs "detail/*.hpp") +file(GLOB detail_srcs "detail/*.cpp") dart_add_core_headers(${hdrs} ${detail_hdrs}) dart_add_core_sources(${srcs} ${detail_srcs}) @@ -23,7 +25,18 @@ install( DESTINATION include/dart/constraint COMPONENT headers ) +install( + FILES ${detail_hdrs} + DESTINATION include/dart/constraint/detail + COMPONENT headers +) dart_format_add( + BoxedLcpSolver.hpp + DantzigBoxedLcpSolver.hpp + DantzigBoxedLcpSolver.cpp + PgsBoxedLcpSolver.hpp + PgsBoxedLcpSolver.cpp SmartPointer.hpp + detail/BoxedLcpSolver-impl.hpp ) diff --git a/dart/constraint/ConstrainedGroup.cpp b/dart/constraint/ConstrainedGroup.cpp index cc78a57edceee..5804fcbdcb104 100644 --- a/dart/constraint/ConstrainedGroup.cpp +++ b/dart/constraint/ConstrainedGroup.cpp @@ -71,7 +71,14 @@ std::size_t ConstrainedGroup::getNumConstraints() const } //============================================================================== -ConstraintBasePtr ConstrainedGroup::getConstraint(std::size_t _index) const +ConstraintBasePtr ConstrainedGroup::getConstraint(std::size_t _index) +{ + assert(_index < mConstraints.size()); + return mConstraints[_index]; +} + +//============================================================================== +ConstConstraintBasePtr ConstrainedGroup::getConstraint(std::size_t _index) const { assert(_index < mConstraints.size()); return mConstraints[_index]; diff --git a/dart/constraint/ConstrainedGroup.hpp b/dart/constraint/ConstrainedGroup.hpp index b3d10af7c070c..60cc811627049 100644 --- a/dart/constraint/ConstrainedGroup.hpp +++ b/dart/constraint/ConstrainedGroup.hpp @@ -78,7 +78,10 @@ class ConstrainedGroup std::size_t getNumConstraints() const; /// Return a constraint - ConstraintBasePtr getConstraint(std::size_t _index) const; + ConstraintBasePtr getConstraint(std::size_t _index); + + /// Return a constraint + ConstConstraintBasePtr getConstraint(std::size_t _index) const; /// Remove constraint void removeConstraint(const ConstraintBasePtr& _constraint); diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 137c974cbf326..80b587c91006a 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -49,8 +49,7 @@ #include "dart/constraint/JointLimitConstraint.hpp" #include "dart/constraint/ServoMotorConstraint.hpp" #include "dart/constraint/JointCoulombFrictionConstraint.hpp" -#include "dart/constraint/DantzigLCPSolver.hpp" -#include "dart/constraint/PGSLCPSolver.hpp" +#include "dart/constraint/LCPSolver.hpp" namespace dart { namespace constraint { @@ -64,8 +63,7 @@ ConstraintSolver::ConstraintSolver(double timeStep) mCollisionOption( collision::CollisionOption( true, 1000u, std::make_shared())), - mTimeStep(timeStep), - mLCPSolver(new DantzigLCPSolver(mTimeStep)) + mTimeStep(timeStep) { assert(timeStep > 0.0); @@ -111,6 +109,12 @@ void ConstraintSolver::addSkeletons(const std::vector& skeletons) addSkeleton(skeleton); } +//============================================================================== +const std::vector& ConstraintSolver::getSkeletons() const +{ + return mSkeletons; +} + //============================================================================== void ConstraintSolver::removeSkeleton(const SkeletonPtr& skeleton) { @@ -134,7 +138,7 @@ void ConstraintSolver::removeSkeleton(const SkeletonPtr& skeleton) void ConstraintSolver::removeSkeletons( const std::vector& skeletons) { - for (const auto skeleton : skeletons) + for (const auto& skeleton : skeletons) removeSkeleton(skeleton); } @@ -146,26 +150,26 @@ void ConstraintSolver::removeAllSkeletons() } //============================================================================== -void ConstraintSolver::addConstraint(const ConstraintBasePtr& _constraint) +void ConstraintSolver::addConstraint(const ConstraintBasePtr& constraint) { - assert(_constraint); + assert(constraint); - if (containConstraint(_constraint)) + if (containConstraint(constraint)) { dtwarn << "Constraint solver already contains constraint that you are " << "trying to add." << std::endl; return; } - mManualConstraints.push_back(_constraint); + mManualConstraints.push_back(constraint); } //============================================================================== -void ConstraintSolver::removeConstraint(const ConstraintBasePtr& _constraint) +void ConstraintSolver::removeConstraint(const ConstraintBasePtr& constraint) { - assert(_constraint); + assert(constraint); - if (!containConstraint(_constraint)) + if (!containConstraint(constraint)) { dtwarn << "Constraint solver deos not contain constraint that you are " << "trying to remove." << std::endl; @@ -173,7 +177,7 @@ void ConstraintSolver::removeConstraint(const ConstraintBasePtr& _constraint) } mManualConstraints.erase(remove(mManualConstraints.begin(), - mManualConstraints.end(), _constraint), + mManualConstraints.end(), constraint), mManualConstraints.end()); } @@ -194,9 +198,6 @@ void ConstraintSolver::setTimeStep(double _timeStep) { assert(_timeStep > 0.0 && "Time step should be positive value."); mTimeStep = _timeStep; - - if (mLCPSolver) - mLCPSolver->setTimeStep(mTimeStep); } //============================================================================== @@ -287,27 +288,33 @@ ConstraintSolver::getLastCollisionResult() const } //============================================================================== -void ConstraintSolver::setLCPSolver(std::unique_ptr _lcpSolver) +void ConstraintSolver::setLCPSolver(std::unique_ptr /*lcpSolver*/) { - assert(_lcpSolver && "Invalid LCP solver."); - - mLCPSolver = std::move(_lcpSolver); + dtwarn << "[ConstraintSolver::setLCPSolver] This function is deprecated in " + << "DART 6.7. Please use " + << "BoxedLcpConstraintSolver::setBoxedLcpSolver() instead. " + << "Doing nothing."; } //============================================================================== LCPSolver* ConstraintSolver::getLCPSolver() const { - return mLCPSolver.get(); + dtwarn << "[ConstraintSolver::getLCPSolver] This function is deprecated in " + << "DART 6.7. Please use " + << "BoxedLcpConstraintSolver::getBoxedLcpSolver() instead. " + << "Returning nullptr."; + + return nullptr; } //============================================================================== void ConstraintSolver::solve() { - for (std::size_t i = 0; i < mSkeletons.size(); ++i) + for (auto& skeleton : mSkeletons) { - mSkeletons[i]->clearConstraintImpulses(); + skeleton->clearConstraintImpulses(); DART_SUPPRESS_DEPRECATED_BEGIN - mSkeletons[i]->clearCollidingBodies(); + skeleton->clearCollidingBodies(); DART_SUPPRESS_DEPRECATED_END } @@ -337,37 +344,35 @@ bool ConstraintSolver::containSkeleton(const ConstSkeletonPtr& _skeleton) const } //============================================================================== -bool ConstraintSolver::checkAndAddSkeleton(const SkeletonPtr& _skeleton) +bool ConstraintSolver::checkAndAddSkeleton(const SkeletonPtr& skeleton) { - if (!containSkeleton(_skeleton)) + if (!containSkeleton(skeleton)) { - mSkeletons.push_back(_skeleton); + mSkeletons.push_back(skeleton); return true; } else { - dtwarn << "Skeleton [" << _skeleton->getName() + dtwarn << "Skeleton [" << skeleton->getName() << "] is already in ConstraintSolver." << std::endl; return false; } } //============================================================================== -bool ConstraintSolver::containConstraint( - const ConstConstraintBasePtr& _constraint) const +bool ConstraintSolver::containConstraint(const ConstConstraintBasePtr& constraint) const { return std::find(mManualConstraints.begin(), mManualConstraints.end(), - _constraint) != mManualConstraints.end(); + constraint) != mManualConstraints.end(); } //============================================================================== -bool ConstraintSolver::checkAndAddConstraint( - const ConstraintBasePtr& _constraint) +bool ConstraintSolver::checkAndAddConstraint(const ConstraintBasePtr& constraint) { - if (!containConstraint(_constraint)) + if (!containConstraint(constraint)) { - mManualConstraints.push_back(_constraint); + mManualConstraints.push_back(constraint); return true; } else @@ -410,9 +415,9 @@ void ConstraintSolver::updateConstraints() // Create new contact constraints for (auto i = 0u; i < mCollisionResult.getNumContacts(); ++i) { - auto& ct = mCollisionResult.getContact(i); + auto& contact = mCollisionResult.getContact(i); - if (collision::Contact::isZeroNormal(ct.normal)) + if (collision::Contact::isZeroNormal(contact.normal)) { // Skip this contact. This is because we assume that a contact with // zero-length normal is invalid. @@ -421,24 +426,24 @@ void ConstraintSolver::updateConstraints() // Set colliding bodies auto shapeFrame1 = const_cast( - ct.collisionObject1->getShapeFrame()); + contact.collisionObject1->getShapeFrame()); auto shapeFrame2 = const_cast( - ct.collisionObject2->getShapeFrame()); + contact.collisionObject2->getShapeFrame()); DART_SUPPRESS_DEPRECATED_BEGIN shapeFrame1->asShapeNode()->getBodyNodePtr()->setColliding(true); shapeFrame2->asShapeNode()->getBodyNodePtr()->setColliding(true); DART_SUPPRESS_DEPRECATED_END - if (isSoftContact(ct)) + if (isSoftContact(contact)) { mSoftContactConstraints.push_back( - std::make_shared(ct, mTimeStep)); + std::make_shared(contact, mTimeStep)); } else { mContactConstraints.push_back( - std::make_shared(ct, mTimeStep)); + std::make_shared(contact, mTimeStep)); } } @@ -491,12 +496,16 @@ DART_SUPPRESS_DEPRECATED_END } if (joint->isPositionLimitEnforced()) + { mJointLimitConstraints.push_back( std::make_shared(joint)); + } if (joint->getActuatorType() == dynamics::Joint::SERVO) + { mServoMotorConstraints.push_back( std::make_shared(joint)); + } } } @@ -539,26 +548,20 @@ void ConstraintSolver::buildConstrainedGroups() //---------------------------------------------------------------------------- // Unite skeletons according to constraints's relationships //---------------------------------------------------------------------------- - for (std::vector::iterator it = mActiveConstraints.begin(); - it != mActiveConstraints.end(); ++it) - { - (*it)->uniteSkeletons(); - } + for (const auto& activeConstraint : mActiveConstraints) + activeConstraint->uniteSkeletons(); //---------------------------------------------------------------------------- // Build constraint groups //---------------------------------------------------------------------------- - for (std::vector::const_iterator it = mActiveConstraints.begin(); - it != mActiveConstraints.end(); ++it) + for (const auto& activeConstraint : mActiveConstraints) { bool found = false; - dynamics::SkeletonPtr skel = (*it)->getRootSkeleton(); + const auto& skel = activeConstraint->getRootSkeleton(); - for (std::vector::const_iterator itConstGroup - = mConstrainedGroups.begin(); - itConstGroup != mConstrainedGroups.end(); ++itConstGroup) + for (const auto& constrainedGroup : mConstrainedGroups) { - if ((*itConstGroup).mRootSkeleton == skel) + if (constrainedGroup.mRootSkeleton == skel) { found = true; break; @@ -575,43 +578,36 @@ void ConstraintSolver::buildConstrainedGroups() } // Add active constraints to constrained groups - for (std::vector::const_iterator it = mActiveConstraints.begin(); - it != mActiveConstraints.end(); ++it) + for (const auto& activeConstraint : mActiveConstraints) { - dynamics::SkeletonPtr skel = (*it)->getRootSkeleton(); - mConstrainedGroups[skel->mUnionIndex].addConstraint(*it); + const auto& skel = activeConstraint->getRootSkeleton(); + mConstrainedGroups[skel->mUnionIndex].addConstraint(activeConstraint); } //---------------------------------------------------------------------------- // Reset union since we don't need union information anymore. //---------------------------------------------------------------------------- - for (std::vector::iterator it = mSkeletons.begin(); - it != mSkeletons.end(); ++it) - { - (*it)->resetUnion(); - } + for (auto& skeleton : mSkeletons) + skeleton->resetUnion(); } //============================================================================== void ConstraintSolver::solveConstrainedGroups() { - for (std::vector::iterator it = mConstrainedGroups.begin(); - it != mConstrainedGroups.end(); ++it) - { - mLCPSolver->solve(&(*it)); - } + for (auto& constraintGroup : mConstrainedGroups) + solveConstrainedGroup(constraintGroup); } //============================================================================== bool ConstraintSolver::isSoftContact(const collision::Contact& contact) const { - auto shapeNode1 = contact.collisionObject1->getShapeFrame()->asShapeNode(); - auto shapeNode2 = contact.collisionObject2->getShapeFrame()->asShapeNode(); + auto* shapeNode1 = contact.collisionObject1->getShapeFrame()->asShapeNode(); + auto* shapeNode2 = contact.collisionObject2->getShapeFrame()->asShapeNode(); assert(shapeNode1); assert(shapeNode2); - auto bodyNode1 = shapeNode1->getBodyNodePtr().get(); - auto bodyNode2 = shapeNode2->getBodyNodePtr().get(); + auto* bodyNode1 = shapeNode1->getBodyNodePtr().get(); + auto* bodyNode2 = shapeNode2->getBodyNodePtr().get(); auto bodyNode1IsSoft = dynamic_cast(bodyNode1) != nullptr; diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp index 4d39cef15397f..6da63948e0b63 100644 --- a/dart/constraint/ConstraintSolver.hpp +++ b/dart/constraint/ConstraintSolver.hpp @@ -61,7 +61,7 @@ class ConstraintSolver /// Copy constructor // TODO: implement copy constructor since this class contains a pointer to // allocated memory. - ConstraintSolver(const ConstraintSolver& _other) = delete; + ConstraintSolver(const ConstraintSolver& other) = delete; /// Destructor virtual ~ConstraintSolver(); @@ -72,6 +72,9 @@ class ConstraintSolver /// Add mutiple skeletons void addSkeletons(const std::vector& skeletons); + /// Returns all the skeletons added to this ConstraintSolver. + const std::vector& getSkeletons() const; + /// Remove single skeleton void removeSkeleton(const dynamics::SkeletonPtr& skeleton); @@ -82,10 +85,10 @@ class ConstraintSolver void removeAllSkeletons(); /// Add a constraint - void addConstraint(const ConstraintBasePtr& _constraint); + void addConstraint(const ConstraintBasePtr& constraint); /// Remove a constraint - void removeConstraint(const ConstraintBasePtr& _constraint); + void removeConstraint(const ConstraintBasePtr& constraint); /// Remove all constraints void removeAllConstraints(); @@ -138,26 +141,31 @@ class ConstraintSolver const collision::CollisionResult& getLastCollisionResult() const; /// Set LCP solver - void setLCPSolver(std::unique_ptr _lcpSolver); + DART_DEPRECATED(6.7) + void setLCPSolver(std::unique_ptr lcpSolver); /// Get LCP solver + DART_DEPRECATED(6.7) LCPSolver* getLCPSolver() const; /// Solve constraint impulses and apply them to the skeletons void solve(); -private: +protected: + // TODO(JS): Docstring + virtual void solveConstrainedGroup(ConstrainedGroup& group) = 0; + /// Check if the skeleton is contained in this solver - bool containSkeleton(const dynamics::ConstSkeletonPtr& _skeleton) const; + bool containSkeleton(const dynamics::ConstSkeletonPtr& skeleton) const; /// Add skeleton if the constraint is not contained in this solver - bool checkAndAddSkeleton(const dynamics::SkeletonPtr& _skeleton); + bool checkAndAddSkeleton(const dynamics::SkeletonPtr& skeleton); /// Check if the constraint is contained in this solver - bool containConstraint(const ConstConstraintBasePtr& _constraint) const; + bool containConstraint(const ConstConstraintBasePtr& constraint) const; /// Add constraint if the constraint is not contained in this solver - bool checkAndAddConstraint(const ConstraintBasePtr& _constraint); + bool checkAndAddConstraint(const ConstraintBasePtr& constraint); /// Update constraints void updateConstraints(); @@ -169,7 +177,7 @@ class ConstraintSolver void solveConstrainedGroups(); /// Return true if at least one of colliding body is soft body - bool isSoftContact(const collision::Contact& _contact) const; + bool isSoftContact(const collision::Contact& contact) const; using CollisionDetector = collision::CollisionDetector; @@ -188,9 +196,6 @@ class ConstraintSolver /// Time step double mTimeStep; - /// LCP solver - std::unique_ptr mLCPSolver; - /// Skeleton list std::vector mSkeletons; diff --git a/dart/constraint/DantzigBoxedLcpSolver.cpp b/dart/constraint/DantzigBoxedLcpSolver.cpp new file mode 100644 index 0000000000000..a92c1d4b28fb8 --- /dev/null +++ b/dart/constraint/DantzigBoxedLcpSolver.cpp @@ -0,0 +1,77 @@ +/* + * Copyright (c) 2011-2018, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/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/constraint/DantzigBoxedLcpSolver.hpp" + +#include "dart/external/odelcpsolver/lcp.h" + +namespace dart { +namespace constraint { + +//============================================================================== +const std::string& DantzigBoxedLcpSolver::getType() const +{ + return getStaticType(); +} + +//============================================================================== +const std::string& DantzigBoxedLcpSolver::getStaticType() +{ + static const std::string type = "DantzigBoxedLcpSolver"; + return type; +} + +//============================================================================== +void DantzigBoxedLcpSolver::solve( + int n, + double* A, + double* x, + double* b, + int /*nub*/, + double* lo, + double* hi, + int* findex) +{ + dSolveLCP(n, A, x, b, nullptr, 0, lo, hi, findex); +} + +#ifndef NDEBUG +//============================================================================== +bool DantzigBoxedLcpSolver::canSolve(int /*n*/, const double* /*A*/) +{ + // TODO(JS): Not implemented. + return true; +} +#endif + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/DantzigBoxedLcpSolver.hpp b/dart/constraint/DantzigBoxedLcpSolver.hpp new file mode 100644 index 0000000000000..155fd06ce574c --- /dev/null +++ b/dart/constraint/DantzigBoxedLcpSolver.hpp @@ -0,0 +1,70 @@ +/* + * Copyright (c) 2011-2018, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/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. + */ + +#ifndef DART_CONSTRAINT_DANTZIGBOXEDLCPSOLVER_HPP_ +#define DART_CONSTRAINT_DANTZIGBOXEDLCPSOLVER_HPP_ + +#include "dart/constraint/BoxedLcpSolver.hpp" + +namespace dart { +namespace constraint { + +class DantzigBoxedLcpSolver : public BoxedLcpSolver +{ +public: + // Documentation inherited. + const std::string& getType() const override; + + /// Returns type for this class + static const std::string& getStaticType(); + + // Documentation inherited. + void solve( + int n, + double* A, + double* x, + double* b, + int nub, + double* lo, + double* hi, + int* findex) override; + +#ifndef NDEBUG + // Documentation inherited. + bool canSolve(int n, const double* A) override; +#endif +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_DANTZIGBOXEDLCPSOLVER_HPP_ diff --git a/dart/constraint/DantzigLCPSolver.cpp b/dart/constraint/DantzigLCPSolver.cpp index 04c45bc0a0895..67b3d34177218 100644 --- a/dart/constraint/DantzigLCPSolver.cpp +++ b/dart/constraint/DantzigLCPSolver.cpp @@ -50,11 +50,13 @@ namespace constraint { //============================================================================== DantzigLCPSolver::DantzigLCPSolver(double _timestep) : LCPSolver(_timestep) { + // Do nothing } //============================================================================== DantzigLCPSolver::~DantzigLCPSolver() { + // Do nothing } //============================================================================== diff --git a/dart/constraint/DantzigLCPSolver.hpp b/dart/constraint/DantzigLCPSolver.hpp index 6a6019f775e04..95ece479ab4e9 100644 --- a/dart/constraint/DantzigLCPSolver.hpp +++ b/dart/constraint/DantzigLCPSolver.hpp @@ -41,6 +41,9 @@ namespace dart { namespace constraint { +/// \deprecated This header has been deprecated in DART 6.7. Please include +/// DantzigBoxedLcpSolver.hpp intead. +/// /// DantzigLCPSolver is a LCP solver that uses ODE's implementation of Dantzig /// algorithm class DantzigLCPSolver : public LCPSolver diff --git a/dart/constraint/LCPSolver.hpp b/dart/constraint/LCPSolver.hpp index 5addf439d28e0..21ddbf0791ca9 100644 --- a/dart/constraint/LCPSolver.hpp +++ b/dart/constraint/LCPSolver.hpp @@ -38,6 +38,8 @@ namespace constraint { class ConstrainedGroup; +/// \deprecated This header has been deprecated in DART 6.7. +/// /// LCPSolver class LCPSolver { diff --git a/dart/constraint/PGSLCPSolver.hpp b/dart/constraint/PGSLCPSolver.hpp index 9f8529c13e6dd..d8173e51545f6 100644 --- a/dart/constraint/PGSLCPSolver.hpp +++ b/dart/constraint/PGSLCPSolver.hpp @@ -41,6 +41,9 @@ namespace dart { namespace constraint { +/// \deprecated This header has been deprecated in DART 6.7. Please include +/// PgsBoxedLcpSolver.hpp intead. +/// /// PGSLCPSolver class PGSLCPSolver : public LCPSolver { diff --git a/dart/constraint/PgsBoxedLcpSolver.cpp b/dart/constraint/PgsBoxedLcpSolver.cpp new file mode 100644 index 0000000000000..9bdf42c66516c --- /dev/null +++ b/dart/constraint/PgsBoxedLcpSolver.cpp @@ -0,0 +1,284 @@ +/* + * Copyright (c) 2011-2018, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/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/constraint/PgsBoxedLcpSolver.hpp" + +#include +#include +#include +#include "dart/external/odelcpsolver/matrix.h" +#include "dart/external/odelcpsolver/misc.h" +#include "dart/math/Constants.hpp" + +#define PGS_EPSILON 10e-9 + +namespace dart { +namespace constraint { + +//============================================================================== +PgsBoxedLcpSolver::Option::Option( + int maxIteration, + double deltaXTolerance, + double relativeDeltaXTolerance, + double epsilonForDivision, + bool randomizeConstraintOrder) + : mMaxIteration(maxIteration), + mDeltaXThreshold(deltaXTolerance), + mRelativeDeltaXTolerance(relativeDeltaXTolerance), + mEpsilonForDivision(epsilonForDivision), + mRandomizeConstraintOrder(randomizeConstraintOrder) +{ + // Do nothing +} + +//============================================================================== +const std::string& PgsBoxedLcpSolver::getType() const +{ + return getStaticType(); +} + +//============================================================================== +const std::string& PgsBoxedLcpSolver::getStaticType() +{ + static const std::string type = "PgsBoxedLcpSolver"; + return type; +} + +void PgsBoxedLcpSolver::solve( + int n, + double* A, + double* x, + double* b, + int nub, + double* lo, + double* hi, + int* findex) +{ + const int nskip = dPAD(n); + + // If all the variables are unbounded then we can just factor, solve, and + // return.R + if (nub >= n) + { + mCacheD.resize(n); + std::fill(mCacheD.begin(), mCacheD.end(), 0); + + dFactorLDLT(A, mCacheD.data(), n, nskip); + dSolveLDLT(A, mCacheD.data(), b, n, nskip); + std::memcpy(x, b, n * sizeof(double)); + + return; + } + + mCacheOrder.clear(); + mCacheOrder.reserve(n); + + bool possibleToTerminate = true; + for (int i = 0; i < n; ++i) + { + // mOrderCacheing + if (A[nskip * i + i] < mOption.mEpsilonForDivision) + { + x[i] = 0.0; + continue; + } + + mCacheOrder.push_back(i); + + // Initial loop + const double* A_ptr = A + nskip * i; + const double old_x = x[i]; + + double new_x = b[i]; + + for (int j = 0; j < i; ++j) + new_x -= A_ptr[j] * x[j]; + + for (int j = i + 1; j < n; ++j) + new_x -= A_ptr[j] * x[j]; + + new_x /= A[nskip * i + i]; + + if (findex[i] >= 0) + { + const double hi_tmp = hi[i] * x[findex[i]]; + const double lo_tmp = -hi_tmp; + + if (new_x > hi_tmp) + x[i] = hi_tmp; + else if (new_x < lo_tmp) + x[i] = lo_tmp; + else + x[i] = new_x; + } + else + { + if (new_x > hi[i]) + x[i] = hi[i]; + else if (new_x < lo[i]) + x[i] = lo[i]; + else + x[i] = new_x; + } + + // Test + if (possibleToTerminate) + { + const double deltaX = std::abs(x[i] - old_x); + if (deltaX > mOption.mDeltaXThreshold) + possibleToTerminate = false; + } + } + + if (possibleToTerminate) + { + // return true; + return; + } + + // Normalizing + for (const auto& index : mCacheOrder) + { + const double dummy = 1.0 / A[nskip * index + index]; + b[index] *= dummy; + for (int j = 0; j < n; ++j) + A[nskip * index + j] *= dummy; + } + + for (int iter = 1; iter < mOption.mMaxIteration; ++iter) + { + if (mOption.mRandomizeConstraintOrder) + { + if ((iter & 7) == 0) + { + for (std::size_t i = 1; i < mCacheOrder.size(); ++i) + { + const int tmp = mCacheOrder[i]; + const int swapi = dRandInt(i + 1); + mCacheOrder[i] = mCacheOrder[swapi]; + mCacheOrder[swapi] = tmp; + } + } + } + + possibleToTerminate = true; + + // Single loop + for (const auto& index : mCacheOrder) + { + const double* A_ptr = A + nskip * index; + double new_x = b[index]; + const double old_x = x[index]; + + for (int j = 0; j < index; j++) + new_x -= A_ptr[j] * x[j]; + + for (int j = index + 1; j < n; j++) + new_x -= A_ptr[j] * x[j]; + + if (findex[index] >= 0) + { + const double hi_tmp = hi[index] * x[findex[index]]; + const double lo_tmp = -hi_tmp; + + if (new_x > hi_tmp) + x[index] = hi_tmp; + else if (new_x < lo_tmp) + x[index] = lo_tmp; + else + x[index] = new_x; + } + else + { + if (new_x > hi[index]) + x[index] = hi[index]; + else if (new_x < lo[index]) + x[index] = lo[index]; + else + x[index] = new_x; + } + + if (possibleToTerminate + && std::abs(x[index]) > mOption.mEpsilonForDivision) + { + const double relativeDeltaX = std::abs((x[index] - old_x) / x[index]); + if (relativeDeltaX > mOption.mRelativeDeltaXTolerance) + possibleToTerminate = false; + } + } + + if (possibleToTerminate) + break; + } + + // return sentinel; // TODO(JS): Consider providing the result passing + // sentinel + // in it. +} + +#ifndef NDEBUG +//============================================================================== +bool PgsBoxedLcpSolver::canSolve(int n, const double* A) +{ + const int nskip = dPAD(n); + + // Return false if A has zero-diagonal or A is nonsymmetric matrix + for (auto i = 0; i < n; ++i) + { + if (A[nskip * i + i] < PGS_EPSILON) + return false; + + for (auto j = 0; j < n; ++j) + { + if (std::abs(A[nskip * i + j] - A[nskip * j + i]) > PGS_EPSILON) + return false; + } + } + + return true; +} +#endif + +//============================================================================== +void PgsBoxedLcpSolver::setOption(const PgsBoxedLcpSolver::Option& option) +{ + mOption = option; +} + +//============================================================================== +const PgsBoxedLcpSolver::Option& PgsBoxedLcpSolver::getOption() const +{ + return mOption; +} + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/PgsBoxedLcpSolver.hpp b/dart/constraint/PgsBoxedLcpSolver.hpp new file mode 100644 index 0000000000000..3dcc3a673c624 --- /dev/null +++ b/dart/constraint/PgsBoxedLcpSolver.hpp @@ -0,0 +1,104 @@ +/* + * Copyright (c) 2011-2018, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/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. + */ + +#ifndef DART_CONSTRAINT_PGSBOXEDLCPSOLVER_HPP_ +#define DART_CONSTRAINT_PGSBOXEDLCPSOLVER_HPP_ + +#include +#include "dart/constraint/BoxedLcpSolver.hpp" + +namespace dart { +namespace constraint { + +/// Implementation of projected Gauss-Seidel (PGS) LCP solver. +class PgsBoxedLcpSolver : public BoxedLcpSolver +{ +public: + struct Option + { + int mMaxIteration; + double mDeltaXThreshold; + double mRelativeDeltaXTolerance; + double mEpsilonForDivision; + bool mRandomizeConstraintOrder; + + Option( + int maxIteration = 30, + double deltaXTolerance = 1e-6, + double relativeDeltaXTolerance = 1e-3, + double epsilonForDivision = 1e-9, + bool randomizeConstraintOrder = false); + }; + + // Documentation inherited. + const std::string& getType() const override; + + /// Returns type for this class + static const std::string& getStaticType(); + + // Documentation inherited. + void solve( + int n, + double* A, + double* x, + double* b, + int nub, + double* lo, + double* hi, + int* findex) override; + +#ifndef NDEBUG + // Documentation inherited. + bool canSolve(int n, const double* A) override; +#endif + + /// Sets options + void setOption(const Option& option); + + /// Returns options. + const Option& getOption() const; + +protected: + Option mOption; + + mutable std::vector mCacheOrder; + mutable std::vector mCacheD; + mutable Eigen::VectorXd mCachedNormalizedA; + mutable Eigen::MatrixXd mCachedNormalizedB; + mutable Eigen::VectorXd mCacheZ; + mutable Eigen::VectorXd mCacheOldX; +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_PGSBOXEDLCPSOLVER_HPP_ diff --git a/dart/constraint/SmartPointer.hpp b/dart/constraint/SmartPointer.hpp index fa4cb835b53a6..7d9bfa7a96ffa 100644 --- a/dart/constraint/SmartPointer.hpp +++ b/dart/constraint/SmartPointer.hpp @@ -38,7 +38,10 @@ namespace dart { namespace constraint { +DART_COMMON_DECLARE_SMART_POINTERS(ConstraintSolver) + DART_COMMON_DECLARE_SHARED_WEAK(ConstrainedGroup) + DART_COMMON_DECLARE_SHARED_WEAK(ConstraintBase) DART_COMMON_DECLARE_SHARED_WEAK(ClosedLoopConstraint) DART_COMMON_DECLARE_SHARED_WEAK(ContactConstraint) @@ -46,9 +49,14 @@ DART_COMMON_DECLARE_SHARED_WEAK(SoftContactConstraint) DART_COMMON_DECLARE_SHARED_WEAK(JointLimitConstraint) DART_COMMON_DECLARE_SHARED_WEAK(ServoMotorConstraint) DART_COMMON_DECLARE_SHARED_WEAK(JointCoulombFrictionConstraint) -DART_COMMON_DECLARE_SHARED_WEAK(JointConstraint) + DART_COMMON_DECLARE_SHARED_WEAK(LCPSolver) +DART_COMMON_DECLARE_SHARED_WEAK(BoxedLcpSolver) +DART_COMMON_DECLARE_SHARED_WEAK(PgsBoxedLcpSolver) +DART_COMMON_DECLARE_SHARED_WEAK(PsorBoxedLcpSolver) +DART_COMMON_DECLARE_SHARED_WEAK(JacobiBoxedLcpSolver) +DART_COMMON_DECLARE_SHARED_WEAK(JointConstraint) DART_COMMON_DECLARE_SHARED_WEAK(BallJointConstraint) DART_COMMON_DECLARE_SHARED_WEAK(WeldJointConstraint) diff --git a/dart/constraint/detail/BoxedLcpSolver-impl.hpp b/dart/constraint/detail/BoxedLcpSolver-impl.hpp new file mode 100644 index 0000000000000..d7a6d38175310 --- /dev/null +++ b/dart/constraint/detail/BoxedLcpSolver-impl.hpp @@ -0,0 +1,50 @@ +/* + * Copyright (c) 2011-2018, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/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. + */ + +#ifndef DART_CONSTRAINT_DETAIL_BOXEDLCPSOLVER_IMPL_HPP_ +#define DART_CONSTRAINT_DETAIL_BOXEDLCPSOLVER_IMPL_HPP_ + +#include "dart/constraint/BoxedLcpSolver.hpp" + +namespace dart { +namespace constraint { + +template +bool BoxedLcpSolver::is() const +{ + return getType() == BoxedLcpSolverT::getStaticType(); +} + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_DETAIL_BOXEDLCPSOLVER_IMPL_HPP_ diff --git a/dart/math/Helpers.hpp b/dart/math/Helpers.hpp index 8d92a66b25995..7806a94e399b4 100644 --- a/dart/math/Helpers.hpp +++ b/dart/math/Helpers.hpp @@ -401,6 +401,16 @@ inline Eigen::Vector3d Black() return Eigen::Vector3d(0.05, 0.05, 0.05); } +inline Eigen::Vector4d LightGray(double alpha) +{ + return Eigen::Vector4d(0.9, 0.9, 0.9, alpha); +} + +inline Eigen::Vector3d LightGray() +{ + return Eigen::Vector3d(0.9, 0.9, 0.9); +} + inline Eigen::Vector4d Gray(double alpha) { return Eigen::Vector4d(0.6, 0.6, 0.6, alpha); diff --git a/dart/simulation/CMakeLists.txt b/dart/simulation/CMakeLists.txt index 3ec09ec84f2ce..0400070d6063d 100644 --- a/dart/simulation/CMakeLists.txt +++ b/dart/simulation/CMakeLists.txt @@ -1,6 +1,8 @@ # Search all header and source files file(GLOB hdrs "*.hpp") file(GLOB srcs "*.cpp") +file(GLOB detail_hdrs "detail/*.hpp") +file(GLOB detail_srcs "detail/*.cpp") dart_add_core_headers(${hdrs} ${detail_hdrs}) dart_add_core_sources(${srcs} ${detail_srcs}) @@ -23,3 +25,12 @@ install( DESTINATION include/dart/simulation COMPONENT headers ) +install( + FILES ${detail_hdrs} + DESTINATION include/dart/simulation/detail + COMPONENT headers +) + +dart_format_add( + detail/World-impl.hpp +) diff --git a/dart/simulation/SmartPointer.hpp b/dart/simulation/SmartPointer.hpp new file mode 100644 index 0000000000000..613e018c4e9ed --- /dev/null +++ b/dart/simulation/SmartPointer.hpp @@ -0,0 +1,46 @@ +/* + * Copyright (c) 2011-2018, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/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. + */ + +#ifndef DART_SIMULATION_SMARTPOINTER_HPP_ +#define DART_SIMULATION_SMARTPOINTER_HPP_ + +#include "dart/common/SmartPointer.hpp" + +namespace dart { +namespace simulation { + +DART_COMMON_DECLARE_SHARED_WEAK(World) + +} // namespace simulation +} // namespace dart + +#endif // DART_SIMULATION_SMARTPOINTER_HPP_ diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 51ff870a224ce..29940e8a70043 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -45,7 +45,7 @@ #include "dart/common/Console.hpp" #include "dart/integration/SemiImplicitEulerIntegrator.hpp" #include "dart/dynamics/Skeleton.hpp" -#include "dart/constraint/ConstraintSolver.hpp" +#include "dart/constraint/BoxedLcpConstraintSolver.hpp" #include "dart/collision/CollisionGroup.hpp" namespace dart { @@ -66,7 +66,8 @@ World::World(const std::string& _name) mTimeStep(0.001), mTime(0.0), mFrame(0), - mConstraintSolver(new constraint::ConstraintSolver(mTimeStep)), + mConstraintSolver( + new constraint::BoxedLcpConstraintSolver(mTimeStep)), mRecording(new Recording(mSkeletons)), onNameChanged(mNameChangedSignal) { @@ -76,7 +77,6 @@ World::World(const std::string& _name) //============================================================================== World::~World() { - delete mConstraintSolver; delete mRecording; for(common::Connection& connection : mNameConnectionsForSkeletons) @@ -528,10 +528,26 @@ const collision::CollisionResult& World::getLastCollisionResult() const return mConstraintSolver->getLastCollisionResult(); } +//============================================================================== +void World::setConstraintSolver(constraint::UniqueConstraintSolverPtr solver) +{ + if (!solver) + { + dtwarn << "[World::setConstraintSolver] nullptr for constraint solver is " + << "not allowed. Doing nothing."; + return; + } + + solver->removeAllSkeletons(); + solver->addSkeletons(mConstraintSolver->getSkeletons()); + + mConstraintSolver = std::move(solver); +} + //============================================================================== constraint::ConstraintSolver* World::getConstraintSolver() const { - return mConstraintSolver; + return mConstraintSolver.get(); } //============================================================================== diff --git a/dart/simulation/World.hpp b/dart/simulation/World.hpp index c3e65384da294..015d65c99309b 100644 --- a/dart/simulation/World.hpp +++ b/dart/simulation/World.hpp @@ -53,6 +53,8 @@ #include "dart/dynamics/Skeleton.hpp" #include "dart/collision/CollisionOption.hpp" #include "dart/simulation/Recording.hpp" +#include "dart/simulation/SmartPointer.hpp" +#include "dart/constraint/SmartPointer.hpp" namespace dart { @@ -85,6 +87,10 @@ class World : public virtual common::Subject = common::Signal; + /// Creates World as shared_ptr + template + static WorldPtr create(Args&&... args); + //-------------------------------------------------------------------------- // Constructor and Destructor //-------------------------------------------------------------------------- @@ -229,6 +235,9 @@ class World : public virtual common::Subject // Constraint //-------------------------------------------------------------------------- + /// Sets the constraint solver + void setConstraintSolver(constraint::UniqueConstraintSolverPtr solver); + /// Get the constraint solver constraint::ConstraintSolver* getConstraintSolver() const; @@ -295,7 +304,7 @@ class World : public virtual common::Subject int mFrame; /// Constraint solver - constraint::ConstraintSolver* mConstraintSolver; + std::unique_ptr mConstraintSolver; /// Recording* mRecording; @@ -316,4 +325,6 @@ class World : public virtual common::Subject } // namespace simulation } // namespace dart +#include "dart/simulation/detail/World-impl.hpp" + #endif // DART_SIMULATION_WORLD_HPP_ diff --git a/dart/simulation/detail/World-impl.hpp b/dart/simulation/detail/World-impl.hpp new file mode 100644 index 0000000000000..68b2285d465c9 --- /dev/null +++ b/dart/simulation/detail/World-impl.hpp @@ -0,0 +1,57 @@ +/* + * Copyright (c) 2011-2017, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/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 code incorporates portions of Open Dynamics Engine + * (Copyright (c) 2001-2004, Russell L. Smith. All rights + * reserved.) and portions of FCL (Copyright (c) 2011, Willow + * Garage, Inc. All rights reserved.), which were released under + * the same BSD license as below + * + * 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. + */ + +#ifndef DART_SIMULATION_DETAIL_WORLD_IMPL_HPP_ +#define DART_SIMULATION_DETAIL_WORLD_IMPL_HPP_ + +#include "dart/simulation/World.hpp" + +namespace dart { +namespace simulation { + +//============================================================================== +template +WorldPtr World::create(Args&&... args) +{ + return std::make_shared(std::forward(args)...); +} + +} // namespace simulation +} // namespace dart + +#endif // DART_SIMULATION_DETAIL_WORLD_IMPL_HPP_ diff --git a/doxygen/Doxyfile.in b/doxygen/Doxyfile.in index 2c67baf35caab..e786f258ce81d 100644 --- a/doxygen/Doxyfile.in +++ b/doxygen/Doxyfile.in @@ -1660,7 +1660,7 @@ PAPER_TYPE = a4wide # If left blank no extra packages will be included. # This tag requires that the tag GENERATE_LATEX is set to YES. -EXTRA_PACKAGES = +EXTRA_PACKAGES = amsmath # The LATEX_HEADER tag can be used to specify a personal LaTeX header for the # generated LaTeX document. The header should contain everything until the first diff --git a/examples/osgExamples/CMakeLists.txt b/examples/osgExamples/CMakeLists.txt index dca1592571903..aa29623ec54c8 100644 --- a/examples/osgExamples/CMakeLists.txt +++ b/examples/osgExamples/CMakeLists.txt @@ -1,5 +1,6 @@ add_subdirectory(osgAtlasPuppet) add_subdirectory(osgAtlasSimbicon) +add_subdirectory(osgBoxStacking) add_subdirectory(osgDragAndDrop) add_subdirectory(osgEmpty) add_subdirectory(osgHuboPuppet) diff --git a/examples/osgExamples/osgBoxStacking/CMakeLists.txt b/examples/osgExamples/osgBoxStacking/CMakeLists.txt new file mode 100644 index 0000000000000..cfbaddd0703ee --- /dev/null +++ b/examples/osgExamples/osgBoxStacking/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.8.12) + +if(DART_IN_SOURCE_BUILD) + include(${CMAKE_CURRENT_SOURCE_DIR}/InSourceBuild.cmake) + return() +endif() + +project(osgBoxStacking) + +find_package(DART 6.5.0 REQUIRED COMPONENTS utils-urdf gui-osg CONFIG) + +add_compile_options(-std=c++11) + +include_directories(${DART_INCLUDE_DIRS}) + +file(GLOB srcs "*.cpp" "*.hpp") +add_executable(${PROJECT_NAME} ${srcs}) + +target_link_libraries(${PROJECT_NAME} ${DART_LIBRARIES}) diff --git a/examples/osgExamples/osgBoxStacking/InSourceBuild.cmake b/examples/osgExamples/osgBoxStacking/InSourceBuild.cmake new file mode 100644 index 0000000000000..2284b1a85226c --- /dev/null +++ b/examples/osgExamples/osgBoxStacking/InSourceBuild.cmake @@ -0,0 +1,18 @@ +get_filename_component(example_name ${CMAKE_CURRENT_LIST_DIR} NAME) + +if(NOT TARGET dart-utils-urdf OR NOT TARGET dart-gui-osg) + return() +endif() + +file(GLOB srcs "*.cpp" "*.hpp") + +add_executable(${example_name} ${srcs}) +target_link_libraries(${example_name} dart dart-utils-urdf dart-gui-osg) +set_target_properties(${example_name} + PROPERTIES + RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin" +) + +dart_add_example(${example_name}) + +dart_format_add(${srcs}) diff --git a/examples/osgExamples/osgBoxStacking/README.md b/examples/osgExamples/osgBoxStacking/README.md new file mode 100644 index 0000000000000..965b52cdf07b2 --- /dev/null +++ b/examples/osgExamples/osgBoxStacking/README.md @@ -0,0 +1,20 @@ +This project is dependent on DART. Please make sure a proper version of DART is +installed before building this project. + +## Build Instructions + +From this directory: + + $ mkdir build + $ cd build + $ cmake .. + $ make + +## Execute Instructions + +Launch the executable from the build directory above: + + $ ./{generated_executable} + +Follow the instructions detailed in the console. + diff --git a/examples/osgExamples/osgBoxStacking/osgBoxStacking.cpp b/examples/osgExamples/osgBoxStacking/osgBoxStacking.cpp new file mode 100644 index 0000000000000..b2cee92f11920 --- /dev/null +++ b/examples/osgExamples/osgBoxStacking/osgBoxStacking.cpp @@ -0,0 +1,442 @@ +/* + * Copyright (c) 2011-2018, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/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 + +#include +#include +#include +#include + +using namespace dart; + +//============================================================================== +dynamics::SkeletonPtr createBox(const Eigen::Vector3d& position) +{ + dynamics::SkeletonPtr boxSkel = dynamics::Skeleton::create("box"); + + // Give the floor a body + dynamics::BodyNodePtr boxBody + = boxSkel->createJointAndBodyNodePair(nullptr) + .second; + + // Give the body a shape + double boxWidth = 1.0; + double boxDepth = 1.0; + double boxHeight = 0.5; + auto boxShape = std::make_shared( + Eigen::Vector3d(boxWidth, boxDepth, boxHeight)); + dynamics::ShapeNode* shapeNode + = boxBody->createShapeNodeWith(boxShape); + shapeNode->getVisualAspect()->setColor(dart::math::randomVector<3>(0.0, 1.0)); + + // Put the body into position + Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); + tf.translation() = position; + boxBody->getParentJoint()->setTransformFromParentBodyNode(tf); + + return boxSkel; +} + +//============================================================================== +std::vector createBoxStack( + std::size_t numBoxes, double heightFromGround = 0.5) +{ + std::vector boxSkels(numBoxes); + + for (auto i = 0u; i < numBoxes; ++i) + boxSkels[i] = createBox( + Eigen::Vector3d(0.0, 0.0, heightFromGround + 0.25 + i * 0.5)); + + return boxSkels; +} + +//============================================================================== +dynamics::SkeletonPtr createFloor() +{ + dynamics::SkeletonPtr floor = dynamics::Skeleton::create("floor"); + + // Give the floor a body + dynamics::BodyNodePtr body + = floor->createJointAndBodyNodePair(nullptr).second; + + // Give the body a shape + double floorWidth = 10.0; + double floorHeight = 0.01; + auto box = std::make_shared( + Eigen::Vector3d(floorWidth, floorWidth, floorHeight)); + dynamics::ShapeNode* shapeNode + = body->createShapeNodeWith(box); + shapeNode->getVisualAspect()->setColor(dart::Color::LightGray()); + + // Put the body into position + Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); + tf.translation() = Eigen::Vector3d(0.0, 0.0, -floorHeight / 2.0); + body->getParentJoint()->setTransformFromParentBodyNode(tf); + + return floor; +} + +//============================================================================== +class CustomWorldNode : public dart::gui::osg::WorldNode +{ +public: + explicit CustomWorldNode(const dart::simulation::WorldPtr& world = nullptr) + : dart::gui::osg::WorldNode(world) + { + // Set up the customized WorldNode + } + + void customPreRefresh() + { + // Use this function to execute custom code before each time that the + // window is rendered. This function can be deleted if it does not need + // to be used. + } + + void customPostRefresh() + { + // Use this function to execute custom code after each time that the + // window is rendered. This function can be deleted if it does not need + // to be used. + } + + void customPreStep() + { + // Use this function to execute custom code before each simulation time + // step is performed. This function can be deleted if it does not need + // to be used. + } + + void customPostStep() + { + // Use this function to execute custom code after each simulation time + // step is performed. This function can be deleted if it does not need + // to be used. + } +}; + +//============================================================================== +class CustomEventHandler : public osgGA::GUIEventHandler +{ +public: + CustomEventHandler(/*Pass in any necessary arguments*/) + { + // Set up the customized event handler + } + + virtual bool handle( + const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter&) override + { + if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN) + { + if (ea.getKey() == 'q') + { + std::cout << "Lowercase q pressed" << std::endl; + return true; + } + else if (ea.getKey() == 'Q') + { + std::cout << "Capital Q pressed" << std::endl; + return true; + } + else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Left) + { + std::cout << "Left arrow key pressed" << std::endl; + return true; + } + else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Right) + { + std::cout << "Right arrow key pressed" << std::endl; + return true; + } + } + else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP) + { + if (ea.getKey() == 'q') + { + std::cout << "Lowercase q released" << std::endl; + return true; + } + else if (ea.getKey() == 'Q') + { + std::cout << "Capital Q released" << std::endl; + return true; + } + else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Left) + { + std::cout << "Left arrow key released" << std::endl; + return true; + } + else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Right) + { + std::cout << "Right arrow key released" << std::endl; + return true; + } + } + + // The return value should be 'true' if the input has been fully handled + // and should not be visible to any remaining event handlers. It should be + // false if the input has not been fully handled and should be viewed by + // any remaining event handlers. + return false; + } +}; + +//============================================================================== +class TestWidget : public dart::gui::osg::ImGuiWidget +{ +public: + /// Constructor + TestWidget( + dart::gui::osg::ImGuiViewer* viewer, dart::simulation::WorldPtr world) + : mViewer(viewer), + mWorld(std::move(world)), + mGuiGravity(true), + mGravity(true), + mGuiHeadlights(true), + mLcpSolverType(0) + { + // Do nothing + } + + // Documentation inherited + void render() override + { + ImGui::SetNextWindowPos(ImVec2(10, 20)); + if (!ImGui::Begin( + "Box Stacking", + nullptr, + ImVec2(240, 320), + 0.5f, + ImGuiWindowFlags_NoResize | ImGuiWindowFlags_MenuBar + | ImGuiWindowFlags_HorizontalScrollbar)) + { + // Early out if the window is collapsed, as an optimization. + ImGui::End(); + return; + } + + // Menu + if (ImGui::BeginMenuBar()) + { + if (ImGui::BeginMenu("Menu")) + { + if (ImGui::MenuItem("Exit")) + mViewer->setDone(true); + ImGui::EndMenu(); + } + if (ImGui::BeginMenu("Help")) + { + if (ImGui::MenuItem("About DART")) + mViewer->showAbout(); + ImGui::EndMenu(); + } + ImGui::EndMenuBar(); + } + + ImGui::Text("Box stacking demo"); + ImGui::Spacing(); + + ImGui::Separator(); + ImGui::Text( + "%.3f ms/frame (%.1f FPS)", + 1000.0f / ImGui::GetIO().Framerate, + ImGui::GetIO().Framerate); + ImGui::Spacing(); + + if (ImGui::CollapsingHeader("Simulation", ImGuiTreeNodeFlags_DefaultOpen)) + { + int e = mViewer->isSimulating() ? 0 : 1; + if (mViewer->isAllowingSimulation()) + { + if (ImGui::RadioButton("Play", &e, 0) && !mViewer->isSimulating()) + mViewer->simulate(true); + ImGui::SameLine(); + if (ImGui::RadioButton("Pause", &e, 1) && mViewer->isSimulating()) + mViewer->simulate(false); + } + + ImGui::Text("LCP solver:"); + + ImGui::RadioButton("Dantzig", &mLcpSolverType, 0); + ImGui::SameLine(); + ImGui::RadioButton("PGS", &mLcpSolverType, 1); + setLcpSolver(mLcpSolverType); + + ImGui::Text("Time: %.3f", mWorld->getTime()); + } + + if (ImGui::CollapsingHeader( + "World Options", ImGuiTreeNodeFlags_DefaultOpen)) + { + // Gravity + ImGui::Checkbox("Gravity On/Off", &mGuiGravity); + setGravity(mGuiGravity); + + ImGui::Spacing(); + + // Headlights + mGuiHeadlights = mViewer->checkHeadlights(); + ImGui::Checkbox("Headlights On/Off", &mGuiHeadlights); + mViewer->switchHeadlights(mGuiHeadlights); + } + + if (ImGui::CollapsingHeader("View", ImGuiTreeNodeFlags_DefaultOpen)) + { + osg::Vec3d eye; + osg::Vec3d center; + osg::Vec3d up; + mViewer->getCamera()->getViewMatrixAsLookAt(eye, center, up); + + ImGui::Text("Eye : (%.2f, %.2f, %.2f)", eye.x(), eye.y(), eye.z()); + ImGui::Text( + "Center: (%.2f, %.2f, %.2f)", center.x(), center.y(), center.z()); + ImGui::Text("Up : (%.2f, %.2f, %.2f)", up.x(), up.y(), up.z()); + } + + if (ImGui::CollapsingHeader("Help")) + { + ImGui::PushTextWrapPos(ImGui::GetCursorPos().x + 320); + ImGui::Text("User Guide:\n"); + ImGui::Text("%s", mViewer->getInstructions().c_str()); + ImGui::PopTextWrapPos(); + } + + ImGui::End(); + } + +protected: + void setLcpSolver(int lcpSolverType) + { + if (lcpSolverType == mLcpSolverType) + return; + + auto constraintSolver = mWorld->getConstraintSolver(); + auto blcpConstraintSolver + = dynamic_cast(constraintSolver); + + if (!constraintSolver) + return; + + auto boxedLcpSolver = blcpConstraintSolver->getBoxedLcpSolver().get(); + + if (lcpSolverType == 0) + { + if (boxedLcpSolver->is()) + return; + + blcpConstraintSolver->setBoxedLcpSolver( + std::make_shared()); + } + else if (lcpSolverType == 1) + { + if (boxedLcpSolver->is()) + return; + + blcpConstraintSolver->setBoxedLcpSolver( + std::make_shared()); + } + else + { + dtwarn << "Unsupported boxed-LCP solver selected: " << lcpSolverType + << "\n"; + } + } + + void setGravity(bool gravity) + { + if (mGravity == gravity) + return; + + mGravity = gravity; + + if (mGravity) + mWorld->setGravity(-9.81 * Eigen::Vector3d::UnitZ()); + else + mWorld->setGravity(Eigen::Vector3d::Zero()); + } + + dart::gui::osg::ImGuiViewer* mViewer; + dart::simulation::WorldPtr mWorld; + bool mGuiGravity; + bool mGravity; + bool mGuiHeadlights; + int mLcpSolverType; +}; + +//============================================================================== +int main() +{ + simulation::WorldPtr world = simulation::World::create(); + world->addSkeleton(createFloor()); + + auto boxSkels = createBoxStack(5); + for (const auto& boxSkel : boxSkels) + world->addSkeleton(boxSkel); + + // Wrap a WorldNode around it + osg::ref_ptr node = new CustomWorldNode(world); + + // Create a Viewer and set it up with the WorldNode + dart::gui::osg::ImGuiViewer viewer; + viewer.addWorldNode(node); + + // Add control widget for atlas + viewer.getImGuiHandler()->addWidget( + std::make_shared(&viewer, world)); + + // Pass in the custom event handler + viewer.addEventHandler(new CustomEventHandler); + + // Set up the window to be 800x640 + viewer.setUpViewInWindow(0, 0, 800, 640); + + // Adjust the viewpoint of the Viewer + viewer.getCameraManipulator()->setHomePosition( + ::osg::Vec3(12.00f, 12.00f, 9.00f), + ::osg::Vec3(0.00f, 0.00f, 2.00f), + ::osg::Vec3(0.00f, 0.00f, 1.00f)); + + // We need to re-dirty the CameraManipulator by passing it into the viewer + // again, so that the viewer knows to update its HomePosition setting + viewer.setCameraManipulator(viewer.getCameraManipulator()); + + // Begin running the application loop + viewer.run(); + + return 0; +} diff --git a/examples/osgExamples/osgImGui/osgImGui.cpp b/examples/osgExamples/osgImGui/osgImGui.cpp index 5b8273abab31a..3324a586a8d37 100644 --- a/examples/osgExamples/osgImGui/osgImGui.cpp +++ b/examples/osgExamples/osgImGui/osgImGui.cpp @@ -32,6 +32,7 @@ #include #include +#include //============================================================================== class CustomWorldNode : public dart::gui::osg::WorldNode @@ -140,7 +141,130 @@ class CustomEventHandler : public osgGA::GUIEventHandler // any remaining event handlers. return false; } +}; + +//============================================================================== +class TestWidget : public dart::gui::osg::ImGuiWidget +{ +public: + /// Constructor + TestWidget( + dart::gui::osg::ImGuiViewer* viewer, dart::simulation::WorldPtr world) + : mViewer(viewer), + mWorld(std::move(world)), + mGuiGravity(true), + mGravity(true), + mGuiHeadlights(true) + { + // Do nothing + } + + // Documentation inherited + void render() override + { + ImGui::SetNextWindowPos(ImVec2(10,20)); + if (!ImGui::Begin("Tinkertoy Control", nullptr, ImVec2(240, 320), 0.5f, + ImGuiWindowFlags_NoResize | + ImGuiWindowFlags_MenuBar | + ImGuiWindowFlags_HorizontalScrollbar)) + { + // Early out if the window is collapsed, as an optimization. + ImGui::End(); + return; + } + + // Menu + if (ImGui::BeginMenuBar()) + { + if (ImGui::BeginMenu("Menu")) + { + if (ImGui::MenuItem("Exit")) + mViewer->setDone(true); + ImGui::EndMenu(); + } + if (ImGui::BeginMenu("Help")) + { + if (ImGui::MenuItem("About DART")) + mViewer->showAbout(); + ImGui::EndMenu(); + } + ImGui::EndMenuBar(); + } + + ImGui::Text("An empty OSG example with ImGui"); + ImGui::Spacing(); + + if (ImGui::CollapsingHeader("Simulation", ImGuiTreeNodeFlags_DefaultOpen)) + { + int e = mViewer->isSimulating() ? 0 : 1; + if(mViewer->isAllowingSimulation()) + { + if (ImGui::RadioButton("Play", &e, 0) && !mViewer->isSimulating()) + mViewer->simulate(true); + ImGui::SameLine(); + if (ImGui::RadioButton("Pause", &e, 1) && mViewer->isSimulating()) + mViewer->simulate(false); + } + ImGui::Text("Time: %.3f", mWorld->getTime()); + } + + if (ImGui::CollapsingHeader("World Options", ImGuiTreeNodeFlags_DefaultOpen)) + { + // Gravity + ImGui::Checkbox("Gravity On/Off", &mGuiGravity); + setGravity(mGuiGravity); + + ImGui::Spacing(); + + // Headlights + mGuiHeadlights = mViewer->checkHeadlights(); + ImGui::Checkbox("Headlights On/Off", &mGuiHeadlights); + mViewer->switchHeadlights(mGuiHeadlights); + } + + if (ImGui::CollapsingHeader("View", ImGuiTreeNodeFlags_DefaultOpen)) + { + osg::Vec3d eye; + osg::Vec3d center; + osg::Vec3d up; + mViewer->getCamera()->getViewMatrixAsLookAt(eye, center, up); + + ImGui::Text("Eye : (%.2f, %.2f, %.2f)", eye.x(), eye.y(), eye.z()); + ImGui::Text("Center: (%.2f, %.2f, %.2f)", center.x(), center.y(), center.z()); + ImGui::Text("Up : (%.2f, %.2f, %.2f)", up.x(), up.y(), up.z()); + } + + if (ImGui::CollapsingHeader("Help")) + { + ImGui::PushTextWrapPos(ImGui::GetCursorPos().x + 320); + ImGui::Text("User Guide:\n"); + ImGui::Text("%s", mViewer->getInstructions().c_str()); + ImGui::PopTextWrapPos(); + } + + ImGui::End(); + } + +protected: + void setGravity(bool gravity) + { + if (mGravity == gravity) + return; + + mGravity = gravity; + + if (mGravity) + mWorld->setGravity(-9.81*Eigen::Vector3d::UnitZ()); + else + mWorld->setGravity(Eigen::Vector3d::Zero()); + } + + dart::gui::osg::ImGuiViewer* mViewer; + dart::simulation::WorldPtr mWorld; + bool mGuiGravity; + bool mGravity; + bool mGuiHeadlights; }; //============================================================================== @@ -151,7 +275,7 @@ int main() // Add a target object to the world dart::gui::osg::InteractiveFramePtr target( - new dart::gui::osg::InteractiveFrame(dart::dynamics::Frame::World())); + new dart::gui::osg::InteractiveFrame(dart::dynamics::Frame::World())); world->addSimpleFrame(target); // Wrap a WorldNode around it @@ -161,6 +285,10 @@ int main() dart::gui::osg::ImGuiViewer viewer; viewer.addWorldNode(node); + // Add control widget for atlas + viewer.getImGuiHandler()->addWidget( + std::make_shared(&viewer, world)); + // Active the drag-and-drop feature for the target viewer.enableDragAndDrop(target.get()); diff --git a/unittests/unit/test_ContactConstraint.cpp b/unittests/unit/test_ContactConstraint.cpp index a5599a8904748..e31c9d891b755 100644 --- a/unittests/unit/test_ContactConstraint.cpp +++ b/unittests/unit/test_ContactConstraint.cpp @@ -42,9 +42,13 @@ using namespace dart; //============================================================================== -TEST(ContactConstraint, ContactWithKinematicJoint) +void testContactWithKinematicJoint( + const constraint::BoxedLcpSolverPtr& lcpSolver, double tol) { auto world = std::make_shared(); + world->setConstraintSolver( + common::make_unique( + world->getTimeStep(), lcpSolver)); auto skeleton1 = dynamics::Skeleton::create("skeleton1"); auto pair1 = skeleton1->createJointAndBodyNodePair(); @@ -82,7 +86,17 @@ TEST(ContactConstraint, ContactWithKinematicJoint) // Need few steps to settle down if (i > 15) { - EXPECT_NEAR(bodyNode2->getLinearVelocity()[0], 0.1, 1e-6); + EXPECT_NEAR(bodyNode2->getLinearVelocity()[0], 0.1, tol); } } } + +//============================================================================== +TEST(ContactConstraint, ContactWithKinematicJoint) +{ + testContactWithKinematicJoint( + std::make_shared(), 1e-6); + + testContactWithKinematicJoint( + std::make_shared(), 1e-4); +}