From 10eb6419b4b289c80774de0231d532e4dc7ef3b0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Michael=20G=C3=B6rner?= <me@v4hn.de>
Date: Sun, 21 Oct 2018 22:47:15 +0200
Subject: [PATCH] Correctly configure fixed TFs in subgroups of
 rigidly-connected links (#912)

Old implementation only computes those transforms *below* the link in the kinematic tree.

If the robot structure contains y-paths made up of fixed transforms, the previous implementation could miss associations.

The structure

- a - b - c
  \
   - d

only added associationed fixed transforms for a and b to c, but missed d.
---
 .../include/moveit/robot_model/robot_model.h  |  1 +
 moveit_core/robot_model/src/robot_model.cpp   | 24 +++---
 moveit_core/robot_model/test/test.cpp         | 75 +++++++++++++++++++
 3 files changed, 90 insertions(+), 10 deletions(-)

diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h
index 15a7f77d30..83ffe0575d 100644
--- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h
+++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h
@@ -436,6 +436,7 @@ class RobotModel
   void setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn>& allocators);
 
 protected:
+  /** \brief Get the transforms between link and all its rigidly attached descendants */
   void computeFixedTransforms(const LinkModel* link, const Eigen::Affine3d& transform,
                               LinkTransformMap& associated_transforms);
 
diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp
index 33caa33db1..284c5fcce2 100644
--- a/moveit_core/robot_model/src/robot_model.cpp
+++ b/moveit_core/robot_model/src/robot_model.cpp
@@ -295,19 +295,23 @@ void RobotModel::buildJointInfo()
     }
   }
 
-  for (std::size_t i = 0; i < link_model_vector_.size(); ++i)
+  bool link_considered[link_model_vector_.size()] = { false };
+  for (const LinkModel* link : link_model_vector_)
   {
+    if (link_considered[link->getLinkIndex()])
+      continue;
+
     LinkTransformMap associated_transforms;
-    computeFixedTransforms(link_model_vector_[i], link_model_vector_[i]->getJointOriginTransform().inverse(),
-                           associated_transforms);
-    if (associated_transforms.size() > 1)
+    computeFixedTransforms(link, link->getJointOriginTransform().inverse(), associated_transforms);
+    for (auto& tf_base : associated_transforms)
     {
-      for (LinkTransformMap::iterator it = associated_transforms.begin(); it != associated_transforms.end(); ++it)
-        if (it->first != link_model_vector_[i])
-        {
-          getLinkModel(it->first->getName())->addAssociatedFixedTransform(link_model_vector_[i], it->second.inverse());
-          link_model_vector_[i]->addAssociatedFixedTransform(it->first, it->second);
-        }
+      link_considered[tf_base.first->getLinkIndex()] = true;
+      for (auto& tf_target : associated_transforms)
+      {
+        if (&tf_base != &tf_target)
+          const_cast<LinkModel*>(tf_base.first)  // regain write access to base LinkModel*
+              ->addAssociatedFixedTransform(tf_target.first, tf_base.second.inverse() * tf_target.second);
+      }
     }
   }
 
diff --git a/moveit_core/robot_model/test/test.cpp b/moveit_core/robot_model/test/test.cpp
index 84744f28c6..2930833c05 100644
--- a/moveit_core/robot_model/test/test.cpp
+++ b/moveit_core/robot_model/test/test.cpp
@@ -102,6 +102,81 @@ TEST_F(LoadPlanningModelsPr2, Model)
   moveit::tools::Profiler::Status();
 }
 
+TEST(SiblingAssociateLinks, SimpleYRobot)
+{
+  /* base_link - a - b - c
+                  \
+                   - d ~ e          */
+  const std::string MODEL = "<?xml version=\"1.0\" ?>"
+                            "<robot name=\"one_robot\">"
+                            "<link name=\"base_link\"/>"
+                            "<joint name=\"joint_a\" type=\"continuous\">"
+                            "  <parent link=\"base_link\"/>"
+                            "  <child link=\"link_a\"/>"
+                            "  <axis xyz=\"0 0 1\"/>"
+                            "  <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
+                            "</joint>"
+                            "<link name=\"link_a\"/>"
+                            "<joint name=\"joint_b\" type=\"fixed\">"
+                            "  <parent link=\"link_a\"/>"
+                            "  <child link=\"link_b\"/>"
+                            "  <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
+                            "</joint>"
+                            "<link name=\"link_b\"/>"
+                            "<joint name=\"joint_c\" type=\"fixed\">"
+                            "  <parent link=\"link_b\"/>"
+                            "  <child link=\"link_c\"/>"
+                            "  <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 0.5 0 \"/>"
+                            "</joint>"
+                            "<link name=\"link_c\"/>"
+                            "<joint name=\"joint_d\" type=\"fixed\">"
+                            "  <parent link=\"link_a\"/>"
+                            "  <child link=\"link_d\"/>"
+                            "  <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
+                            "</joint>"
+                            "<link name=\"link_d\"/>"
+                            "<joint name=\"joint_e\" type=\"continuous\">"
+                            "  <parent link=\"link_d\"/>"
+                            "  <child link=\"link_e\"/>"
+                            "  <axis xyz=\"0 0 1\"/>"
+                            "  <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
+                            "</joint>"
+                            "<link name=\"link_e\"/>"
+                            "</robot>";
+
+  const std::string SMODEL =
+      "<?xml version=\"1.0\" ?>"
+      "<robot name=\"one_robot\">"
+      "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom\" type=\"planar\"/>"
+      "<group name=\"base_joint\"><joint name=\"base_joint\"/></group>"
+      "</robot>";
+  urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL);
+  srdf::ModelSharedPtr srdf_model(new srdf::Model());
+  srdf_model->initString(*urdf_model, SMODEL);
+  moveit::core::RobotModelConstPtr robot_model;
+  robot_model.reset(new moveit::core::RobotModel(urdf_model, srdf_model));
+
+  const std::string a = "link_a", b = "link_b", c = "link_c", d = "link_d";
+  auto connected = { a, b, c, d };  // these are rigidly connected with each other
+  moveit::core::LinkTransformMap map;
+
+  for (const std::string& root : connected)
+  {
+    SCOPED_TRACE("root: " + root);
+    std::set<std::string> expected_set(connected);
+    expected_set.erase(root);
+    std::set<std::string> actual_set;
+    for (const auto& item : robot_model->getLinkModel(root)->getAssociatedFixedTransforms())
+      actual_set.insert(item.first->getName());
+
+    std::ostringstream expected, actual;
+    std::copy(expected_set.begin(), expected_set.end(), std::ostream_iterator<std::string>(expected, " "));
+    std::copy(actual_set.begin(), actual_set.end(), std::ostream_iterator<std::string>(actual, " "));
+
+    EXPECT_EQ(expected.str(), actual.str());
+  }
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);