Update motion planners to account for Joint and State Waypoints unord…

…ered joints relative to kinematics

tesseract/tesseract_common/include/tesseract_common/manipulator_info.h

@@ -48,20 +48,20 @@ class ToolCenterPoint
   /**
    * @brief Tool Center Point Defined by name
    * @param name The tool center point name
-   * @param external The external TCP is used when the robot is holding the part verus the tool.
-   * @note External should also be set to true your kinematic object which includes say a robot and
-   * positioner, and the positioner has the tool and the robot is holding the part. Basically
+   * @param external The external TCP is used when the robot is holding the part versus the tool.
+   * @note External should also be set to true when your kinematic object includes a robot and
+   * positioner, where the positioner has the tool and the robot is holding the part. Basically
    * anytime the tool is not attached to the tip link of the kinematic chain.
    */
   ToolCenterPoint(const std::string& name, bool external = false);
 
   /**
    * @brief Tool Center Point Defined by transform
    * @param transform The tool center point transform
-   * @param external The external TCP is used when the robot is holding the part verus the tool.
+   * @param external The external TCP is used when the robot is holding the part versus the tool.
    * @param external_frame If empty assumed relative to world, otherwise the provided tcp is relative to this link.
-   * @note External should also be set to true your kinematic object which includes say a robot and
-   * positioner, and the positioner has the tool and the robot is holding the part. Basically
+   * @note External should also be set to true when your kinematic object includes a robot and
+   * positioner, where the positioner has the tool and the robot is holding the part. Basically
    * anytime the tool is not attached to the tip link of the kinematic chain.
    */
   ToolCenterPoint(const Eigen::Isometry3d& transform, bool external = false, std::string external_frame = "");
@@ -86,9 +86,9 @@ class ToolCenterPoint
 
   /**
    * @brief Check if tool center point is and external tcp which mean it is not defined
-   * @details The external TCP is used when the robot is holding the part verus the tool.
-   * External should also be set to true your kinematic object which includes say a robot and
-   * positioner, and the positioner has the tool and the robot is holding the part. Basically
+   * @details The external TCP is used when the robot is holding the part versus the tool.
+   * External should also be set to true when your kinematic object includes a robot and
+   * positioner, where the positioner has the tool and the robot is holding the part. Basically
    * anytime the tool is not attached to the tip link of the kinematic chain.
    * @return True if and external TCP, otherwise the false
    */
@@ -137,9 +137,9 @@ class ToolCenterPoint
   Eigen::Isometry3d transform_;
 
   /**
-   * @brief The external TCP is used when the robot is holding the part verus the tool.
-   * @details External should also be set to true your kinematic object which includes say a robot and
-   * positioner, and the positioner has the tool and the robot is holding the part. Basically
+   * @brief The external TCP is used when the robot is holding the part versus the tool.
+   * @details External should also be set to true when your kinematic object includes a robot and
+   * positioner, where the positioner has the tool and the robot is holding the part. Basically
    * anytime the tool is not attached to the tip link of the kinematic chain.
    */
   bool external_{ false };

tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/utils/utils.h

@@ -67,6 +67,34 @@ const std::vector<std::string>& getJointNames(const Waypoint& waypoint);
  */
 Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, const Waypoint& waypoint);
 
+/**
+ * @brief Format the waypoints joint ordered by the provided joint names
+ *
+ * Throws if waypoint does not directly contain that information
+ *
+ * Also this is an expensive call so the motion planners do not leverage this and they expect the order through out
+ * the program all match.
+ *
+ * @param joint_names The joint names defining the order desired
+ * @param waypoint The waypoint to format
+ * @return True if formating was required, otherwise false.
+ */
+bool formatJointPosition(const std::vector<std::string>& joint_names, Waypoint& waypoint);
+
+/**
+ * @brief Check the waypoints joint order against the provided joint names
+ *
+ * Throws if waypoint does not directly contain that information
+ *
+ * Also this is an expensive call so the motion planners do not leverage this and they expect the order through out
+ * the program all match.
+ *
+ * @param joint_names The joint names defining the order desired
+ * @param waypoint The waypoint to check format
+ * @return True if waypoint format is correct, otherwise false.
+ */
+bool checkJointPositionFormat(const std::vector<std::string>& joint_names, const Waypoint& waypoint);
+
 /**
  * @brief Set the joint position for waypoints that contain that information
  * @param waypoint Waypoint to set

tesseract/tesseract_planning/tesseract_command_language/src/utils/utils.cpp

@@ -66,7 +66,7 @@ Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, co
   if (isJointWaypoint(waypoint))
   {
     const auto* jwp = waypoint.cast_const<JointWaypoint>();
-    jv = *jwp;
+    jv = jwp->waypoint;
     jn = jwp->joint_names;
   }
   else if (isStateWaypoint(waypoint))
@@ -80,7 +80,7 @@ Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, co
     throw std::runtime_error("Unsupported waypoint type.");
   }
 
-  if (jn.size() == joint_names.size())
+  if (jn.size() != joint_names.size())
     throw std::runtime_error("Joint name sizes do not match!");
 
   if (joint_names == jn)
@@ -103,6 +103,64 @@ Eigen::VectorXd getJointPosition(const std::vector<std::string>& joint_names, co
   return output;
 }
 
+bool formatJointPosition(const std::vector<std::string>& joint_names, Waypoint& waypoint)
+{
+  Eigen::VectorXd* jv;
+  std::vector<std::string>* jn;
+  if (isJointWaypoint(waypoint))
+  {
+    auto* jwp = waypoint.cast<JointWaypoint>();
+    jv = &(jwp->waypoint);
+    jn = &(jwp->joint_names);
+  }
+  else if (isStateWaypoint(waypoint))
+  {
+    auto* swp = waypoint.cast<StateWaypoint>();
+    jv = &(swp->position);
+    jn = &(swp->joint_names);
+  }
+  else
+  {
+    throw std::runtime_error("Unsupported waypoint type.");
+  }
+
+  if (jn->size() != joint_names.size())
+    throw std::runtime_error("Joint name sizes do not match!");
+
+  if (joint_names == *jn)
+    return false;
+
+  Eigen::VectorXd output = *jv;
+  for (std::size_t i = 0; i < joint_names.size(); ++i)
+  {
+    if (joint_names[i] == (*jn)[i])
+      continue;
+
+    auto it = std::find(jn->begin(), jn->end(), joint_names[i]);
+    if (it == jn->end())
+      throw std::runtime_error("Joint names do not match!");
+
+    long idx = std::distance(jn->begin(), it);
+    output(static_cast<long>(i)) = (*jv)(static_cast<long>(idx));
+  }
+
+  *jn = joint_names;
+  *jv = output;
+
+  return true;
+}
+
+bool checkJointPositionFormat(const std::vector<std::string>& joint_names, const Waypoint& waypoint)
+{
+  if (isJointWaypoint(waypoint))
+    return (joint_names == waypoint.cast_const<JointWaypoint>()->joint_names);
+
+  if (isStateWaypoint(waypoint))
+    return (joint_names == waypoint.cast_const<StateWaypoint>()->joint_names);
+
+  throw std::runtime_error("Unsupported waypoint type.");
+}
+
 bool setJointPosition(Waypoint& waypoint, const Eigen::Ref<const Eigen::VectorXd>& position)
 {
   if (isJointWaypoint(waypoint))

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/core/utils.h

@@ -183,6 +183,14 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
 CompositeInstruction generateNaiveSeed(const CompositeInstruction& composite_instructions,
                                        const tesseract_environment::Environment& env);
 
+/**
+ * @brief This formats the joint and state waypoints to align with the kinematics object
+ * @param composite_instructions The input program to format
+ * @param env The environment information
+ * @return True if the program required formating.
+ */
+bool formatProgram(CompositeInstruction& composite_instructions, const tesseract_environment::Environment& env);
+
 }  // namespace tesseract_planning
 
 #endif  // TESSERACT_PLANNING_UTILS_H

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp

@@ -139,6 +139,10 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
   auto results_flattened = flattenProgramToPattern(response.results, request.instructions);
   auto instructions_flattened = flattenProgram(request.instructions);
 
+  // Check inverse and forward kinematic objects
+  if (problem->manip_fwd_kin->getJointNames() != problem->manip_inv_kin->getJointNames())
+    throw std::runtime_error("Forward and Inverse Kinematic objects joints are not ordered the same!");
+
   // Loop over the flattened results and add them to response if the input was a plan instruction
   Eigen::Index dof = problem->manip_fwd_kin->numJoints();
   Eigen::Index result_index = 0;
@@ -157,7 +161,9 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
         auto* move_instruction = results_flattened[idx].get().cast<MoveInstruction>();
 
         Eigen::Map<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> temp(solution.data() + dof * result_index++, dof);
-        move_instruction->getWaypoint().cast<StateWaypoint>()->position = temp.template cast<double>();
+        auto* swp = move_instruction->getWaypoint().cast<StateWaypoint>();
+        swp->position = temp.template cast<double>();
+        assert(swp->joint_names == problem->manip_fwd_kin->getJointNames());
       }
       else if (plan_instruction->isLinear())
       {
@@ -168,8 +174,9 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
         {
           Eigen::Map<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> temp(solution.data() + dof * result_index++,
                                                                              dof);
-          instruction.cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>()->position =
-              temp.template cast<double>();
+          auto* swp = instruction.cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>();
+          swp->position = temp.template cast<double>();
+          assert(swp->joint_names == problem->manip_fwd_kin->getJointNames());
         }
       }
       else if (plan_instruction->isFreespace())
@@ -190,8 +197,11 @@ tesseract_common::StatusCode DescartesMotionPlanner<FloatType>::solve(const Plan
 
         assert(temp.cols() == static_cast<long>(move_instructions->size()) + 1);
         for (std::size_t i = 0; i < move_instructions->size(); ++i)
-          (*move_instructions)[i].cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>()->position =
-              temp.col(static_cast<long>(i) + 1);
+        {
+          auto* swp = (*move_instructions)[i].cast<MoveInstruction>()->getWaypoint().cast<StateWaypoint>();
+          swp->position = temp.col(static_cast<long>(i) + 1);
+          assert(swp->joint_names == problem->manip_fwd_kin->getJointNames());
+        }
       }
       else
       {

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/problem_generators/default_problem_generator.h

@@ -134,6 +134,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
   }
   else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
   {
+    assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), start_waypoint));
     const Eigen::VectorXd& position = getJointPosition(start_waypoint);
     cur_plan_profile->apply(*prob, position, *start_instruction, composite_mi, active_links, index);
   }
@@ -187,6 +188,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
           }
           else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
           {
+            assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), start_waypoint));
             const Eigen::VectorXd& position = getJointPosition(start_waypoint);
             if (!prob->manip_fwd_kin->calcFwdKin(prev_pose, position))
               throw std::runtime_error("DescartesMotionPlannerConfig: failed to solve forward kinematics!");
@@ -214,6 +216,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
         }
         else if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
         {
+          assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), plan_instruction->getWaypoint()));
           const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
           Eigen::Isometry3d cur_pose = Eigen::Isometry3d::Identity();
           if (!prob->manip_fwd_kin->calcFwdKin(cur_pose, cur_position))
@@ -228,6 +231,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
           }
           else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
           {
+            assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), start_waypoint));
             const Eigen::VectorXd& position = getJointPosition(start_waypoint);
             if (!prob->manip_fwd_kin->calcFwdKin(prev_pose, position))
               throw std::runtime_error("DescartesMotionPlannerConfig: failed to solve forward kinematics!");
@@ -262,6 +266,7 @@ DefaultDescartesProblemGenerator(const std::string& name,
       {
         if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
         {
+          assert(checkJointPositionFormat(prob->manip_fwd_kin->getJointNames(), plan_instruction->getWaypoint()));
           const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
 
           // Descartes does not support freespace so it will only include the plan instruction state, then in