Add method to environment to get relative link transform

tesseract_environment/include/tesseract_environment/environment.h

@@ -403,6 +403,14 @@ class Environment
    */
   Eigen::Isometry3d getLinkTransform(const std::string& link_name) const;
 
+  /**
+   * @brief Get transform between two links using the current state
+   * @param from_link_name The link name the transform should be relative to
+   * @param to_link_name The link name to get transform
+   * @return The relative transform = inv(Transform(from_link_name)) * Transform(to_link_name)
+   */
+  Eigen::Isometry3d getRelativeLinkTransform(const std::string& from_link_name, const std::string& to_link_name) const;
+
   /**
    * @brief Returns a clone of the environments state solver
    *

tesseract_environment/src/environment.cpp

@@ -668,6 +668,13 @@ Eigen::Isometry3d Environment::getLinkTransform(const std::string& link_name) co
   return current_state_.link_transforms.at(link_name);
 }
 
+Eigen::Isometry3d Environment::getRelativeLinkTransform(const std::string& from_link_name,
+                                                        const std::string& to_link_name) const
+{
+  std::shared_lock<std::shared_mutex> lock(mutex_);
+  return current_state_.link_transforms.at(from_link_name).inverse() * current_state_.link_transforms.at(to_link_name);
+}
+
 tesseract_scene_graph::StateSolver::UPtr Environment::getStateSolver() const
 {
   std::shared_lock<std::shared_mutex> lock(mutex_);

tesseract_environment/test/tesseract_environment_unit.cpp

@@ -146,6 +146,19 @@ void runGetLinkTransformsTest(Environment& env)
           env_state.link_transforms.at(link_names.at(i)).isApprox(env.getLinkTransform(link_names.at(i)), 1e-6));
     }
   }
+
+  // Check relative link transform
+  std::vector<std::string> link_names = env.getLinkNames();
+  SceneState env_state = env.getState();
+  for (const auto& link1 : link_names)
+  {
+    for (const auto& link2 : link_names)
+    {
+      Eigen::Isometry3d t1 = env_state.link_transforms.at(link1).inverse() * env_state.link_transforms.at(link2);
+      Eigen::Isometry3d t2 = env.getRelativeLinkTransform(link1, link2);
+      EXPECT_TRUE(t1.isApprox(t2, 1e-6));
+    }
+  }
 }
 
 enum class EnvironmentInitType