Kinematic loops in dart not working with dart 6.7

[osrf-migration]

Original report (archived issue) by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

Since the release of dart 6.7, which included a refactor of the ConstraintSolver, our jenkins builds have shown two test failures of INTEGRATION_kinematic_loop for the dart test variant and compiler warnings complaining about use of deprecated syntax in DARTPhysics.cc.

• https://build.osrfoundation.org/job/gazebo-ci-default-xenial-amd64-gpu-none/385/testReport/
• https://build.osrfoundation.org/job/gazebo-ci-default-xenial-amd64-gpu-none/385/warnings22Result/

I've tried the following to update the syntax based on an example in the refactoring pull request, which fixes the deprecation warnings but not the test failure. Opening the test/worlds/anchored_loop.world shows that the loop is not connected.

diff -r f570b5ae8ebf136e7cf138059b9aa173b05ad720 gazebo/physics/dart/DARTPhysics.cc
--- a/gazebo/physics/dart/DARTPhysics.cc        Mon Mar 11 00:59:07 2019 -0700
+++ b/gazebo/physics/dart/DARTPhysics.cc        Mon Mar 11 01:09:06 2019 -0700
@@ -591,15 +591,17 @@
 
   if (_type == "dantzig")
   {
-    this->dataPtr->dtWorld->getConstraintSolver()->setLCPSolver(
-        dart::common::make_unique<dart::constraint::DantzigLCPSolver>(
-        this->dataPtr->dtWorld->getTimeStep()));
+    this->dataPtr->dtWorld->setConstraintSolver(
+        dart::common::make_unique<dart::constraint::BoxedLcpConstraintSolver>(
+        this->dataPtr->dtWorld->getTimeStep(),
+        std::make_shared<dart::constraint::DantzigBoxedLcpSolver>()));
   }
   else if (_type == "pgs")
   {
-    this->dataPtr->dtWorld->getConstraintSolver()->setLCPSolver(
-        dart::common::make_unique<dart::constraint::PGSLCPSolver>(
-        this->dataPtr->dtWorld->getTimeStep()));
+    this->dataPtr->dtWorld->setConstraintSolver(
+        dart::common::make_unique<dart::constraint::BoxedLcpConstraintSolver>(
+        this->dataPtr->dtWorld->getTimeStep(),
+        std::make_shared<dart::constraint::PgsBoxedLcpSolver>()));
   }
   else
   {

Any advice from @jslee02 or @mxgrey would be appreciated.

[osrf-migration]

Original comment by Jeongseok Lee (Bitbucket: jlee02, GitHub: jslee02).

---

The root cause of this issue is that World::setConstraintSolver() only register skeletons but doesn't transfer the dynamic joint constraints (e.g., kinematic loop constraint), which is a bug. Let me fix this on DART side.

In the meantime, a quick fix without changing DART code would be changing the boxed LCP solver without creating a new constraint solver something like:

auto solver = this->dataPtr->dtWorld->getConstraintSolver();
if (auto boxedLCPSolver = std::dynamic_pointer_cast<dart::constraint::BoxedLcpConstraintSolver>(solver))
{
  boxedLCPSolver->setBoxedLcpSolver(
    std::make_shared<dart::constraint::DantzigBoxedLcpSolver>()));
}

[osrf-migration]

Original comment by Jeongseok Lee (Bitbucket: jlee02, GitHub: jslee02).

---

This should be fixed by dartsim/dart#1260.

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

testing with the branch in dart PR 1260:

• https://build.osrfoundation.org/job/gazebo-any_dart4-devel-xenial-amd64-gpu-nvidia_docker/45/

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

I've already tested it locally, and it fixes the test when using the patch I posted in the issue description. Thanks for the quick fix!

I had some small trouble with the code snippet you posted, as it doesn't like to use dynamic_pointer_cast with the raw pointer solver, but that shouldn't be too hard to figure out.

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

I've started a branch with ifdef's for the different versions of dart. I just need working code for dart 6.7

• https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/branch/issue_2605_9?dest=gazebo9#diff

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

testing issue_2605_9 branch:

• https://build.osrfoundation.org/job/gazebo-any_dart4-devel-xenial-amd64-gpu-nvidia_docker/46/

[osrf-migration]

Original comment by Jeongseok Lee (Bitbucket: jlee02, GitHub: jslee02).

---

I've already tested it locally, and it fixes the test when using the patch I posted in the issue description. Thanks for the quick fix!

Glad it worked!

I had some small trouble with the code snippet you posted, as it doesn't like to use dynamic_pointer_cast with the raw pointer solver, but that shouldn't be too hard to figure out.

Ah, I should have warned you that the code is not tested. 😈

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

see pull request #3086

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

• changed state from "new" to "resolved"

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

→ <<cset bee6908>>