Fix kinematic group working frames and updated so the joint names alw…

…ays align with the same joint group

tesseract_environment/include/tesseract_environment/environment.h

@@ -176,6 +176,13 @@ class Environment
    */
   tesseract_scene_graph::SceneGraph::ConstPtr getSceneGraph() const;
 
+  /**
+   * @brief Get a groups joint names
+   * @param group_name The group name
+   * @return A vector of joint names
+   */
+  std::vector<std::string> getGroupJointNames(const std::string& group_name) const;
+
   /**
    * @brief Get a joint group by name
    * @param group_name The group name

tesseract_environment/src/environment.cpp

@@ -304,14 +304,14 @@ bool Environment::applyCommand(Command::ConstPtr command) { return applyCommands
 
 tesseract_scene_graph::SceneGraph::ConstPtr Environment::getSceneGraph() const { return scene_graph_const_; }
 
-tesseract_kinematics::JointGroup::UPtr Environment::getJointGroup(const std::string& group_name) const
+std::vector<std::string> Environment::getGroupJointNames(const std::string& group_name) const
 {
   std::shared_lock<std::shared_mutex> lock(mutex_);
   auto it =
       std::find(kinematics_information_.group_names.begin(), kinematics_information_.group_names.end(), group_name);
-  // TODO add error message
+
   if (it == kinematics_information_.group_names.end())
-    return nullptr;
+    throw std::runtime_error("Environment, Joint group '" + group_name + "' does not exist!");
 
   auto chain_it = kinematics_information_.chain_groups.find(group_name);
   if (chain_it != kinematics_information_.chain_groups.end())
@@ -322,38 +322,40 @@ tesseract_kinematics::JointGroup::UPtr Environment::getJointGroup(const std::str
     tesseract_scene_graph::ShortestPath path =
         scene_graph_const_->getShortestPath(chain_it->second.begin()->first, chain_it->second.begin()->second);
 
-    return std::make_unique<tesseract_kinematics::JointGroup>(
-        group_name, path.active_joints, *scene_graph_const_, current_state_);
+    return path.active_joints;
   }
 
   auto joint_it = kinematics_information_.joint_groups.find(group_name);
   if (joint_it != kinematics_information_.joint_groups.end())
-    return std::make_unique<tesseract_kinematics::JointGroup>(
-        group_name, joint_it->second, *scene_graph_const_, current_state_);
+    return joint_it->second;
 
   auto link_it = kinematics_information_.link_groups.find(group_name);
   if (link_it != kinematics_information_.link_groups.end())
     throw std::runtime_error("Environment, Link groups are currently not supported!");
 
-  return nullptr;
+  throw std::runtime_error("Environment, failed to get group '" + group_name + "' joint names!");
+}
+
+tesseract_kinematics::JointGroup::UPtr Environment::getJointGroup(const std::string& name) const
+{
+  std::shared_lock<std::shared_mutex> lock(mutex_);
+  std::vector<std::string> joint_names = getGroupJointNames(name);
+  return getJointGroup(name, joint_names);
 }
 
-tesseract_kinematics::JointGroup::UPtr Environment::getJointGroup(const std::string& name,
+tesseract_kinematics::JointGroup::UPtr Environment::getJointGroup(const std::string& group_name,
                                                                   const std::vector<std::string>& joint_names) const
 {
   std::shared_lock<std::shared_mutex> lock(mutex_);
-  return std::make_unique<tesseract_kinematics::JointGroup>(name, joint_names, *scene_graph_const_, current_state_);
+  return std::make_unique<tesseract_kinematics::JointGroup>(
+      group_name, joint_names, *scene_graph_const_, current_state_);
 }
 
 tesseract_kinematics::KinematicGroup::UPtr Environment::getKinematicGroup(const std::string& group_name,
                                                                           std::string ik_solver_name) const
 {
   std::shared_lock<std::shared_mutex> lock(mutex_);
-  auto it =
-      std::find(kinematics_information_.group_names.begin(), kinematics_information_.group_names.end(), group_name);
-  // TODO add error message
-  if (it == kinematics_information_.group_names.end())
-    return nullptr;
+  std::vector<std::string> joint_names = getGroupJointNames(group_name);
 
   if (ik_solver_name.empty())
     ik_solver_name = kinematics_factory_.getDefaultInvKinPlugin(group_name);
@@ -366,7 +368,7 @@ tesseract_kinematics::KinematicGroup::UPtr Environment::getKinematicGroup(const
     return nullptr;
 
   return std::make_unique<tesseract_kinematics::KinematicGroup>(
-      group_name, std::move(inv_kin), *scene_graph_const_, current_state_);
+      group_name, joint_names, std::move(inv_kin), *scene_graph_const_, current_state_);
 }
 
 // NOLINTNEXTLINE

tesseract_kinematics/core/include/tesseract_kinematics/core/kinematic_group.h

@@ -99,6 +99,7 @@ class KinematicGroup : public JointGroup
    * @param scene_state The scene state
    */
   KinematicGroup(std::string name,
+                 std::vector<std::string> joint_names,
                  InverseKinematics::UPtr inv_kin,
                  const tesseract_scene_graph::SceneGraph& scene_graph,
                  const tesseract_scene_graph::SceneState& scene_state);
@@ -135,6 +136,9 @@ class KinematicGroup : public JointGroup
   std::vector<std::string> getAllPossibleTipLinkNames() const;
 
 private:
+  std::vector<std::string> joint_names_;
+  bool reorder_required_{ false };
+  std::vector<Eigen::Index> inv_kin_joint_map_;
   InverseKinematics::UPtr inv_kin_;
   Eigen::Isometry3d inv_to_fwd_base_{ Eigen::Isometry3d::Identity() };
   std::vector<std::string> working_frames_;

tesseract_kinematics/core/src/kinematic_group.cpp

@@ -36,26 +36,41 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 namespace tesseract_kinematics
 {
 KinematicGroup::KinematicGroup(std::string name,
+                               std::vector<std::string> joint_names,
                                InverseKinematics::UPtr inv_kin,
                                const tesseract_scene_graph::SceneGraph& scene_graph,
                                const tesseract_scene_graph::SceneState& scene_state)
-  : JointGroup(std::move(name), inv_kin->getJointNames(), scene_graph, scene_state)
+  : JointGroup(std::move(name), joint_names, scene_graph, scene_state), joint_names_(std::move(joint_names))
 {
   inv_kin_ = std::move(inv_kin);
+  std::vector<std::string> inv_kin_joint_names = inv_kin_->getJointNames();
+
+  if (static_cast<Eigen::Index>(joint_names_.size()) != inv_kin_->numJoints())
+    throw std::runtime_error("KinematicGroup: joint_names is not the correct size");
+
+  if (!tesseract_common::isIdentical(joint_names_, inv_kin_joint_names, false))
+    throw std::runtime_error("KinematicGroup: joint_names does not match same names in inverse kinematics object!");
+
+  reorder_required_ = !tesseract_common::isIdentical(joint_names_, inv_kin_joint_names, true);
+
+  if (reorder_required_)
+  {
+    inv_kin_joint_map_.reserve(joint_names_.size());
+    for (const auto& joint_name : joint_names_)
+      inv_kin_joint_map_.push_back(std::distance(
+          inv_kin_joint_names.begin(), std::find(inv_kin_joint_names.begin(), inv_kin_joint_names.end(), joint_name)));
+  }
 
   std::vector<std::string> active_link_names = state_solver_->getActiveLinkNames();
   std::string working_frame = inv_kin_->getWorkingFrame();
+
+  std::vector<std::string> child_link_names = scene_graph.getLinkChildrenNames(working_frame);
+  working_frames_.push_back(working_frame);
+  working_frames_.insert(working_frames_.end(), child_link_names.begin(), child_link_names.end());
+
   auto it = std::find(active_link_names.begin(), active_link_names.end(), working_frame);
   if (it == active_link_names.end())
-  {
     working_frames_.insert(working_frames_.end(), static_link_names_.begin(), static_link_names_.end());
-  }
-  else
-  {
-    std::vector<std::string> child_link_names = scene_graph.getLinkChildrenNames(working_frame);
-    working_frames_.push_back(working_frame);
-    working_frames_.insert(working_frames_.end(), child_link_names.begin(), child_link_names.end());
-  }
 
   for (const auto& tip_link : inv_kin_->getTipLinkNames())
   {
@@ -73,6 +88,9 @@ KinematicGroup::KinematicGroup(const KinematicGroup& other) : JointGroup(other)
 KinematicGroup& KinematicGroup::operator=(const KinematicGroup& other)
 {
   JointGroup::operator=(other);
+  joint_names_ = other.joint_names_;
+  reorder_required_ = other.reorder_required_;
+  inv_kin_joint_map_ = other.inv_kin_joint_map_;
   inv_kin_ = other.inv_kin_->clone();
   inv_to_fwd_base_ = other.inv_to_fwd_base_;
   working_frames_ = other.working_frames_;
@@ -111,6 +129,29 @@ IKSolutions KinematicGroup::calcInvKin(const KinGroupIKInputs& tip_link_poses,
     ik_inputs[ik_solver_tip_link] = wf_to_tl;
   }
 
+  // format seed for inverse kinematic solver
+  if (reorder_required_)
+  {
+    Eigen::VectorXd ordered_seed = seed;
+    for (Eigen::Index i = 0; i < inv_kin_->numJoints(); ++i)
+      ordered_seed(inv_kin_joint_map_[static_cast<std::size_t>(i)]) = ordered_seed(i);
+
+    IKSolutions solutions = inv_kin_->calcInvKin(ik_inputs, ordered_seed);
+    IKSolutions solutions_filtered;
+    solutions_filtered.reserve(solutions.size());
+    for (const auto& solution : solutions)
+    {
+      Eigen::VectorXd ordered_sol = solution;
+      for (Eigen::Index i = 0; i < inv_kin_->numJoints(); ++i)
+        ordered_sol(i) = solution(inv_kin_joint_map_[static_cast<std::size_t>(i)]);
+
+      if (tesseract_common::satisfiesPositionLimits(ordered_sol, limits_.joint_limits))
+        solutions_filtered.push_back(ordered_sol);
+    }
+
+    return solutions_filtered;
+  }
+
   IKSolutions solutions = inv_kin_->calcInvKin(ik_inputs, seed);
   IKSolutions solutions_filtered;
   solutions_filtered.reserve(solutions.size());

tesseract_kinematics/test/kinematics_test_utils.h

@@ -900,7 +900,7 @@ inline void runInvKinIIWATest(const tesseract_kinematics::KinematicsPluginFactor
 
     runInvKinTest(*inv_kin, *fwd_kin, pose, tip_link_name, seed);
 
-    KinematicGroup kin_group(manip_name, std::move(inv_kin), *scene_graph, scene_state);
+    KinematicGroup kin_group(manip_name, joint_names, std::move(inv_kin), *scene_graph, scene_state);
     EXPECT_EQ(kin_group.getBaseLinkName(), scene_graph->getRoot());
     runInvKinTest(kin_group, pose, base_link_name, tip_link_name, seed);
     runKinGroupJacobianIIWATest(kin_group);
@@ -936,7 +936,7 @@ inline void runInvKinIIWATest(const tesseract_kinematics::KinematicsPluginFactor
 
     runInvKinTest(*inv_kin3, *fwd_kin3, pose, tip_link_name, seed);
 
-    KinematicGroup kin_group(manip_name, std::move(inv_kin3), *scene_graph, scene_state);
+    KinematicGroup kin_group(manip_name, joint_names, std::move(inv_kin3), *scene_graph, scene_state);
     EXPECT_EQ(kin_group.getBaseLinkName(), scene_graph->getRoot());
     runInvKinTest(kin_group, pose, base_link_name, tip_link_name, seed);
     runKinGroupJacobianIIWATest(kin_group);

tesseract_kinematics/test/rep_kinematics_unit.cpp

@@ -142,7 +142,7 @@ TEST(TesseractKinematicsUnit, RobotWithExternalPositionerInverseKinematicUnit)
   EXPECT_EQ(inv_kin->getTipLinkNames()[0], tip_link_name);
   EXPECT_EQ(inv_kin->getJointNames(), joint_names);
 
-  KinematicGroup kin_group(manip_name, std::move(inv_kin), *scene_graph, scene_state);
+  KinematicGroup kin_group(manip_name, joint_names, std::move(inv_kin), *scene_graph, scene_state);
   KinematicGroup kin_group_copy(kin_group);
 
   EXPECT_EQ(kin_group.getBaseLinkName(), scene_graph->getRoot());
@@ -182,7 +182,7 @@ TEST(TesseractKinematicsUnit, RobotWithExternalPositionerInverseKinematicUnit)
   EXPECT_EQ(inv_kin2->getTipLinkNames()[0], tip_link_name);
   EXPECT_EQ(inv_kin2->getJointNames(), joint_names);
 
-  KinematicGroup kin_group2(manip_name, std::move(inv_kin2), *scene_graph, scene_state);
+  KinematicGroup kin_group2(manip_name, joint_names, std::move(inv_kin2), *scene_graph, scene_state);
   EXPECT_EQ(kin_group2.getBaseLinkName(), scene_graph->getRoot());
   runInvKinTest(kin_group2, pose, working_frame, tip_link_name, seed);
   runKinGroupJacobianABBExternalPositionerTest(kin_group2);

tesseract_kinematics/test/rop_kinematics_unit.cpp

@@ -142,7 +142,7 @@ TEST(TesseractKinematicsUnit, RobotOnPositionerInverseKinematicUnit)  // NOLINT
 
   runInvKinTest(*inv_kin, *fwd_kin, pose, tip_link_name, seed);
 
-  KinematicGroup kin_group(manip_name, std::move(inv_kin), *scene_graph, scene_state);
+  KinematicGroup kin_group(manip_name, joint_names, std::move(inv_kin), *scene_graph, scene_state);
   KinematicGroup kin_group_copy(kin_group);
 
   EXPECT_EQ(kin_group.getBaseLinkName(), scene_graph->getRoot());
@@ -155,7 +155,7 @@ TEST(TesseractKinematicsUnit, RobotOnPositionerInverseKinematicUnit)  // NOLINT
   EXPECT_EQ(kin_group.getJointNames(), joint_names);
   EXPECT_EQ(kin_group.getAllPossibleTipLinkNames().size(), 1);
   EXPECT_EQ(kin_group.getAllPossibleTipLinkNames()[0], tip_link_name);
-  EXPECT_EQ(kin_group.getAllValidWorkingFrames().size(), 1);
+  EXPECT_EQ(kin_group.getAllValidWorkingFrames().size(), 12);
   EXPECT_EQ(kin_group.getAllValidWorkingFrames()[0], base_link_name);
 
   // Check KinematicGroup copy
@@ -169,7 +169,7 @@ TEST(TesseractKinematicsUnit, RobotOnPositionerInverseKinematicUnit)  // NOLINT
   EXPECT_EQ(kin_group_copy.getJointNames(), joint_names);
   EXPECT_EQ(kin_group_copy.getAllPossibleTipLinkNames().size(), 1);
   EXPECT_EQ(kin_group_copy.getAllPossibleTipLinkNames()[0], tip_link_name);
-  EXPECT_EQ(kin_group_copy.getAllValidWorkingFrames().size(), 1);
+  EXPECT_EQ(kin_group_copy.getAllValidWorkingFrames().size(), 12);
   EXPECT_EQ(kin_group_copy.getAllValidWorkingFrames()[0], base_link_name);
 
   // Check cloned
@@ -184,7 +184,7 @@ TEST(TesseractKinematicsUnit, RobotOnPositionerInverseKinematicUnit)  // NOLINT
 
   runInvKinTest(*inv_kin2, *fwd_kin, pose, tip_link_name, seed);
 
-  KinematicGroup kin_group2(manip_name, std::move(inv_kin2), *scene_graph, scene_state);
+  KinematicGroup kin_group2(manip_name, joint_names, std::move(inv_kin2), *scene_graph, scene_state);
   EXPECT_EQ(kin_group2.getBaseLinkName(), scene_graph->getRoot());
   runInvKinTest(kin_group2, pose, base_link_name, tip_link_name, seed);
   runKinGroupJacobianABBOnPositionerTest(kin_group2);