Fix bug in getClosestJointSolution in simple planner utils

tesseract-robotics · Dec 8, 2021 · ace80e0 · ace80e0
1 parent e4afe13
commit ace80e0
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 5 deletions.
diff --git a/tesseract_command_language/include/tesseract_command_language/utils/utils.h b/tesseract_command_language/include/tesseract_command_language/utils/utils.h
@@ -39,12 +39,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_common/joint_state.h>
 #include <tesseract_common/types.h>
-#include <tesseract_common/joint_state.h>
 
 namespace tesseract_planning
 {
 /**
- * @brief Convert composite intruction to a joint trajectory
+ * @brief Convert composite instruction to a joint trajectory
  * @param composite_instructions The composite instruction to convert
  * @return A joint trajectory
  */
@@ -79,7 +78,7 @@ const std::vector<std::string>& getJointNames(const Waypoint& waypoint);
  *
  * @param joint_names The joint names defining the order desired
  * @param waypoint The waypoint to
- * @return The joint values ordered by the provied joint_names
+ * @return The joint values ordered by the provided joint_names
  */
 Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, const Waypoint& waypoint);
 
@@ -93,7 +92,7 @@ Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, co
  *
  * @param joint_names The joint names defining the order desired
  * @param waypoint The waypoint to format
- * @return True if formating was required, otherwise false.
+ * @return True if formatting was required, otherwise false.
  */
 bool formatJointPosition(const std::vector<std::string>& joint_names, Waypoint& waypoint);
 

diff --git a/tesseract_motion_planners/core/src/simple/profile/simple_planner_utils.cpp b/tesseract_motion_planners/core/src/simple/profile/simple_planner_utils.cpp
@@ -210,6 +210,7 @@ Eigen::VectorXd getClosestJointSolution(const KinematicGroupInstructionInfo& inf
         if (jp_final.rows() == 0)
         {
           jp_final = solution;
+          dist = (solution - seed).norm();
           continue;
         }
 

diff --git a/tesseract_process_managers/test/tesseract_process_managers_unit.cpp b/tesseract_process_managers/test/tesseract_process_managers_unit.cpp
@@ -212,7 +212,7 @@ TEST_F(TesseractProcessManagerUnit, RasterSimpleMotionPlannerLVSPlanProfileTest)
 
   // The first plan instruction is the start instruction and every other plan instruction should be converted into
   // ten move instruction.
-  EXPECT_EQ(161, mcnt);
+  EXPECT_EQ(98, mcnt);
   EXPECT_TRUE(response.results.hasStartInstruction());
   EXPECT_FALSE(response.results.getManipulatorInfo().empty());
 }