Merged in issue_2605_9 (pull request #3086)

Fix #2605: kinematic loops for DART 6.7 and later Approved-by: Jeongseok Lee <jslee02@gmail.com> Approved-by: Ian Chen <ichen@osrfoundation.org>
gazebosim · Mar 22, 2019 · bee6908 · bee6908
2 parents c46accc + 8aa275f
commit bee6908
Show file tree

Hide file tree

Showing 2 changed files with 40 additions and 0 deletions.
diff --git a/Changelog.md b/Changelog.md
@@ -2,6 +2,10 @@
 
 ## Gazebo 9.X.X (20XX-XX-XX)
 
+1. Fix kinematic loops for DART 6.7 and later
+    * [Pull request 3086](https://bitbucket.org/osrf/gazebo/pull-request/3086)
+    * [Issue 2605](https://bitbucket.org/osrf/gazebo/issues/2605)
+
 1. Port introspection manager performance fix
     * [Pull request 3074](https://bitbucket.org/osrf/gazebo/pull-request/3074)
 

diff --git a/gazebo/physics/dart/DARTPhysics.cc b/gazebo/physics/dart/DARTPhysics.cc
@@ -591,15 +591,51 @@ void DARTPhysics::SetSolverType(const std::string &_type)
 
   if (_type == "dantzig")
   {
+    // DART constraint solver refactored in 6.7, see issue 2605
+    // https://bitbucket.org/osrf/gazebo/issues/2605
+#if DART_MAJOR_MINOR_VERSION_AT_MOST(6, 6)
     this->dataPtr->dtWorld->getConstraintSolver()->setLCPSolver(
         dart::common::make_unique<dart::constraint::DantzigLCPSolver>(
         this->dataPtr->dtWorld->getTimeStep()));
+#elif DART_MAJOR_MINOR_VERSION_AT_MOST(6, 7)
+    auto boxedLCPSolver =
+        dynamic_cast<dart::constraint::BoxedLcpConstraintSolver*>(
+        this->dataPtr->dtWorld->getConstraintSolver());
+    if (boxedLCPSolver)
+    {
+      boxedLCPSolver->setBoxedLcpSolver(
+          std::make_shared<dart::constraint::DantzigBoxedLcpSolver>());
+    }
+#else
+    this->dataPtr->dtWorld->setConstraintSolver(
+        dart::common::make_unique<dart::constraint::BoxedLcpConstraintSolver>(
+        this->dataPtr->dtWorld->getTimeStep(),
+        std::make_shared<dart::constraint::DantzigBoxedLcpSolver>()));
+#endif
   }
   else if (_type == "pgs")
   {
+    // DART constraint solver refactored in 6.7, see issue 2605
+    // https://bitbucket.org/osrf/gazebo/issues/2605
+#if DART_MAJOR_MINOR_VERSION_AT_MOST(6, 6)
     this->dataPtr->dtWorld->getConstraintSolver()->setLCPSolver(
         dart::common::make_unique<dart::constraint::PGSLCPSolver>(
         this->dataPtr->dtWorld->getTimeStep()));
+#elif DART_MAJOR_MINOR_VERSION_AT_MOST(6, 7)
+    auto boxedLCPSolver =
+        dynamic_cast<dart::constraint::BoxedLcpConstraintSolver*>(
+        this->dataPtr->dtWorld->getConstraintSolver());
+    if (boxedLCPSolver)
+    {
+      boxedLCPSolver->setBoxedLcpSolver(
+          std::make_shared<dart::constraint::PgsBoxedLcpSolver>());
+    }
+#else
+    this->dataPtr->dtWorld->setConstraintSolver(
+        dart::common::make_unique<dart::constraint::BoxedLcpConstraintSolver>(
+        this->dataPtr->dtWorld->getTimeStep(),
+        std::make_shared<dart::constraint::PgsBoxedLcpSolver>()));
+#endif
   }
   else
   {