Kinematic loops in dart not working with dart 6.7

Issue #2605 resolved
Steven Peters
created an issue

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.

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 2584fc852e5d 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 @Jeongseok Lee or @Michael Grey would be appreciated.

Comments (9)

  1. Jeongseok Lee

    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>()));
    }
    
  2. Steven Peters reporter

    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.

  3. Jeongseok Lee

    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. 😈

  4. Log in to comment