From 5794e51f684b5361b8e7d5daefc02ce14cd8b426 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Mon, 11 Mar 2019 13:39:31 -0700
Subject: [PATCH 1/4] handle refactored dart constraint solver, add ifdefs for
 dart 6.8 with code that works with dart PR 1260

---
 gazebo/physics/dart/DARTPhysics.cc | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/gazebo/physics/dart/DARTPhysics.cc b/gazebo/physics/dart/DARTPhysics.cc
index 8a7fb26eef..e881b69d01 100644
--- a/gazebo/physics/dart/DARTPhysics.cc
+++ b/gazebo/physics/dart/DARTPhysics.cc
@@ -591,15 +591,35 @@ 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)
+#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)
+#else
+    this->dataPtr->dtWorld->setConstraintSolver(
+        dart::common::make_unique<dart::constraint::BoxedLcpConstraintSolver>(
+        this->dataPtr->dtWorld->getTimeStep(),
+        std::make_shared<dart::constraint::PgsBoxedLcpSolver>()));
+#endif
   }
   else
   {

From bbe47af2df5be03a66bc3b2a48ab80a3c40cebe4 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Tue, 12 Mar 2019 12:00:32 -0700
Subject: [PATCH 2/4] workaround for dart 6.7

---
 gazebo/physics/dart/DARTPhysics.cc | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/gazebo/physics/dart/DARTPhysics.cc b/gazebo/physics/dart/DARTPhysics.cc
index e881b69d01..942f4ed14d 100644
--- a/gazebo/physics/dart/DARTPhysics.cc
+++ b/gazebo/physics/dart/DARTPhysics.cc
@@ -598,6 +598,14 @@ void DARTPhysics::SetSolverType(const std::string &_type)
         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>(
@@ -614,6 +622,14 @@ void DARTPhysics::SetSolverType(const std::string &_type)
         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>(

From 29afe3d7eb70c6cd3e202ef27a813242ab2a506f Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Tue, 12 Mar 2019 12:06:48 -0700
Subject: [PATCH 3/4] changelog

---
 Changelog.md | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/Changelog.md b/Changelog.md
index fa471fb740..f83c2ec936 100644
--- 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. Windows: fix test compilation
     * [Pull request 3082](https://bitbucket.org/osrf/gazebo/pull-request/3082)
 

From 8aa275f9047af4cea016cd8b5524e3366fbe7f8e Mon Sep 17 00:00:00 2001
From: Steven Peters <scpeters@osrfoundation.org>
Date: Fri, 22 Mar 2019 23:13:22 +0000
Subject: [PATCH 4/4] Close branch issue_2605_9