From 91ab5c7cdb801946ff5fc047fece1857a2614ca5 Mon Sep 17 00:00:00 2001
From: Ioan Sucan <isucan@willowgarage.com>
Date: Thu, 12 Apr 2012 23:51:49 +0300
Subject: [PATCH] improve detection of valid  chains

---
 srdf/src/model.cpp | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/srdf/src/model.cpp b/srdf/src/model.cpp
index b0180ee..27a755b 100644
--- a/srdf/src/model.cpp
+++ b/srdf/src/model.cpp
@@ -179,13 +179,26 @@ void srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElemen
       }
       bool found = false;
       boost::shared_ptr<const urdf::Link> l = urdf_model.getLink(tip_str);
+      std::set<std::string> seen;
       while (!found && l)
       {
+        seen.insert(l->name);
         if (l->name == base_str)
           found = true;
         else
           l = l->getParent();
       }
+      if (!found)
+      {
+        l = urdf_model.getLink(base_str);
+        while (!found && l)
+        {
+          if (seen.find(l->name) != seen.end())
+            found = true;
+          else
+            l = l->getParent();
+        }
+      }
       if (found)
         g.chains_.push_back(std::make_pair(base_str, tip_str));
       else