diff --git a/moveit_core/robot_model/test/test.cpp b/moveit_core/robot_model/test/test.cpp
index 84744f28c61..77cb22dc5e3 100644
--- a/moveit_core/robot_model/test/test.cpp
+++ b/moveit_core/robot_model/test/test.cpp
@@ -102,6 +102,85 @@ TEST_F(LoadPlanningModelsPr2, Model)
moveit::tools::Profiler::Status();
}
+TEST(SiblingAssociateLinks, SimpleYRobot)
+{
+ static const std::string MODEL2 =
+ ""
+ ""
+ ""
+ " "
+ " "
+ " "
+ " "
+ " "
+ " "
+ ""
+ ""
+ " "
+ " "
+ " "
+ " "
+ ""
+ ""
+ " "
+ " "
+ " "
+ " "
+ " "
+ " "
+ ""
+ ""
+ " "
+ " "
+ " "
+ ""
+ ""
+ " "
+ " "
+ " "
+ " "
+ " "
+ " "
+ ""
+ " "
+ " "
+ " "
+ " "
+ " "
+ ""
+ " "
+ " "
+ " "
+ " "
+ " "
+ " "
+ ""
+ "";
+
+ static const std::string SMODEL2 =
+ ""
+ ""
+ ""
+ ""
+ ""
+ ""
+ ""
+ ""
+ "";
+ urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2);
+ srdf::ModelSharedPtr srdf_model(new srdf::Model());
+ srdf_model->initString(*urdf_model, SMODEL2);
+ moveit::core::RobotModelConstPtr robot_model;
+ robot_model.reset(new moveit::core::RobotModel(urdf_model, srdf_model));
+
+ const moveit::core::LinkTransformMap map = robot_model->getLinkModel("link_c")->getAssociatedFixedTransforms();
+ // Should contain both link_a and link_b.
+ ASSERT_EQ(map.size(), 2);
+ auto it = map.find(robot_model->getLinkModel("link_b"));
+ // If it correctly finds the sibling link, it will not equal the end iterator.
+ ASSERT_NE(it, map.end());
+}
+
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);