Add path profile to plan and move instruction and modify simple plan …

…profile interface

tesseract_command_language/include/tesseract_command_language/move_instruction.h

@@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_command_language/core/instruction.h>
 #include <tesseract_command_language/core/waypoint.h>
 #include <tesseract_command_language/null_waypoint.h>
+#include <tesseract_command_language/plan_instruction.h>
 #include <tesseract_command_language/constants.h>
 #include <tesseract_command_language/profile_dictionary.h>
 #include <tesseract_command_language/types.h>
@@ -53,11 +54,43 @@ class MoveInstruction
 {
 public:
   MoveInstruction() = default;  // Required for boost serialization do not use
+
+  /**
+   * @brief Plan Instruction Constructor
+   * @details This constructor automatically assigns the transition profile based on the type of motion.
+   * If the motion is LINEAR/CIRCULAR it assigns the transition_profile to the defined profile.
+   * If the motion is FREESPACE/START it is left empty.
+   * @param waypoint The waypoint associated with the instruction
+   * @param type The type of instruction (LINEAR, FREESPACE, CIRGULAR, START)
+   * @param profile The waypoint profile.
+   * @param manipulator_info Then manipulator information
+   */
   MoveInstruction(Waypoint waypoint,
                   MoveInstructionType type,
                   std::string profile = DEFAULT_PROFILE_KEY,
                   ManipulatorInfo manipulator_info = ManipulatorInfo());
 
+  /**
+   * @brief Move Instruction Constructor
+   * @param waypoint The waypoint associated with the instruction
+   * @param type The type of instruction (LINEAR, FREESPACE, CIRGULAR, START)
+   * @param profile The waypoint profile.
+   * @param path_profile The waypoint path profile.
+   * @param manipulator_info Then manipulator information
+   */
+  MoveInstruction(Waypoint waypoint,
+                  MoveInstructionType type,
+                  std::string profile,
+                  std::string path_profile,
+                  ManipulatorInfo manipulator_info = ManipulatorInfo());
+
+  /**
+   * @brief Create a move instruction using properties of the plan_instruction
+   * @param waypoint The waypoint associated with the instruction
+   * @param plan_instruction The plan instruction to copy data from
+   */
+  MoveInstruction(Waypoint waypoint, const PlanInstruction& plan_instruction);
+
   void setWaypoint(Waypoint waypoint);
   Waypoint& getWaypoint();
   const Waypoint& getWaypoint() const;
@@ -69,6 +102,9 @@ class MoveInstruction
   void setProfile(const std::string& profile);
   const std::string& getProfile() const;
 
+  void setPathProfile(const std::string& profile);
+  const std::string& getPathProfile() const;
+
   /** @brief Dictionary of profiles that will override named profiles for a specific task*/
   ProfileDictionary::Ptr profile_overrides;
 
@@ -100,6 +136,9 @@ class MoveInstruction
   /** @brief The profile used for this move instruction */
   std::string profile_{ DEFAULT_PROFILE_KEY };
 
+  /** @brief The profile used for the path to this move instruction */
+  std::string path_profile_;
+
   /** @brief The assigned waypoint (Cartesian or Joint) */
   Waypoint waypoint_{ NullWaypoint() };
 

tesseract_command_language/include/tesseract_command_language/plan_instruction.h

@@ -49,15 +49,46 @@ enum class PlanInstructionType : int
   START = 3
 };
 
+/**
+ * @brief The plan instruction is used when generating motion planning requests
+ * @details
+ * The profile is used to define a set of costs/constraints on the waypoint
+ * The path profile is used to define a set of costs/constraints on the transition waypoints to this waypoint
+ */
 class PlanInstruction
 {
 public:
   PlanInstruction() = default;  // Required for boost serialization do not use
+
+  /**
+   * @brief Plan Instruction Constructor
+   * @details This constructor automatically assigns the transition profile based on the type of motion.
+   * If the motion is LINEAR/CIRCULAR it assigns the transition_profile to the defined profile.
+   * If the motion is FREESPACE/START it is left empty.
+   * @param waypoint The waypoint associated with the instruction
+   * @param type The type of instruction (LINEAR, FREESPACE, CIRGULAR, START)
+   * @param profile The waypoint profile.
+   * @param manipulator_info Then manipulator information
+   */
   PlanInstruction(Waypoint waypoint,
                   PlanInstructionType type,
                   std::string profile = DEFAULT_PROFILE_KEY,
                   ManipulatorInfo manipulator_info = ManipulatorInfo());
 
+  /**
+   * @brief Plan Instruction Constructor
+   * @param waypoint The waypoint associated with the instruction
+   * @param type The type of instruction (LINEAR, FREESPACE, CIRGULAR, START)
+   * @param profile The waypoint profile.
+   * @param path_profile The waypoint path profile.
+   * @param manipulator_info Then manipulator information
+   */
+  PlanInstruction(Waypoint waypoint,
+                  PlanInstructionType type,
+                  std::string profile,
+                  std::string path_profile,
+                  ManipulatorInfo manipulator_info = ManipulatorInfo());
+
   void setWaypoint(Waypoint waypoint);
   Waypoint& getWaypoint();
   const Waypoint& getWaypoint() const;
@@ -69,6 +100,9 @@ class PlanInstruction
   void setProfile(const std::string& profile);
   const std::string& getProfile() const;
 
+  void setPathProfile(const std::string& profile);
+  const std::string& getPathProfile() const;
+
   /** @brief Dictionary of profiles that will override named profiles for a specific task*/
   ProfileDictionary::Ptr profile_overrides;
 
@@ -102,6 +136,9 @@ class PlanInstruction
   /** @brief The profile used for this plan instruction */
   std::string profile_{ DEFAULT_PROFILE_KEY };
 
+  /** @brief The profile used for the path to this plan instruction */
+  std::string path_profile_;
+
   /** @brief Contains information about the manipulator associated with this instruction*/
   ManipulatorInfo manipulator_info_;
 

tesseract_command_language/src/move_instruction.cpp

@@ -44,9 +44,62 @@ MoveInstruction::MoveInstruction(Waypoint waypoint,
   , profile_(std::move(profile))
   , waypoint_(std::move(waypoint))
   , manipulator_info_(std::move(manipulator_info))
+{
+  if (!isStateWaypoint(waypoint_))
+    CONSOLE_BRIDGE_logWarn("MoveInstruction usually expects to be provided a State Waypoint!");
+}
+
+MoveInstruction::MoveInstruction(Waypoint waypoint,
+                                 MoveInstructionType type,
+                                 std::string profile,
+                                 std::string path_profile,
+                                 ManipulatorInfo manipulator_info)
+  : move_type_(type)
+  , profile_(std::move(profile))
+  , path_profile_(std::move(path_profile))
+  , waypoint_(std::move(waypoint))
+  , manipulator_info_(std::move(manipulator_info))
 {
 }
 
+MoveInstruction::MoveInstruction(Waypoint waypoint, const PlanInstruction& plan_instruction)
+  : waypoint_(std::move(waypoint))
+{
+  switch (plan_instruction.getPlanType())
+  {
+    case PlanInstructionType::LINEAR:
+    {
+      move_type_ = MoveInstructionType::LINEAR;
+      break;
+    }
+    case PlanInstructionType::FREESPACE:
+    {
+      move_type_ = MoveInstructionType::FREESPACE;
+      break;
+    }
+    case PlanInstructionType::CIRCULAR:
+    {
+      move_type_ = MoveInstructionType::CIRCULAR;
+      break;
+    }
+    case PlanInstructionType::START:
+    {
+      move_type_ = MoveInstructionType::START;
+      break;
+    }
+    default:
+    {
+      throw std::runtime_error("MoveInstruction, unable to convert plan type to move type!");
+    }
+  }
+
+  profile_ = plan_instruction.getProfile();
+  path_profile_ = plan_instruction.getPathProfile();
+  manipulator_info_ = plan_instruction.getManipulatorInfo();
+  description_ = plan_instruction.getDescription();
+  profile_overrides = plan_instruction.profile_overrides;
+}
+
 void MoveInstruction::setWaypoint(Waypoint waypoint)
 {
   if (!isStateWaypoint(waypoint))
@@ -68,6 +121,9 @@ void MoveInstruction::setProfile(const std::string& profile)
 }
 const std::string& MoveInstruction::getProfile() const { return profile_; }
 
+void MoveInstruction::setPathProfile(const std::string& profile) { path_profile_ = profile; }
+const std::string& MoveInstruction::getPathProfile() const { return path_profile_; }
+
 const std::string& MoveInstruction::getDescription() const { return description_; }
 
 void MoveInstruction::setDescription(const std::string& description) { description_ = description; }
@@ -97,7 +153,8 @@ bool MoveInstruction::operator==(const MoveInstruction& rhs) const
   equal &= (static_cast<int>(move_type_) == static_cast<int>(rhs.move_type_));
   equal &= (waypoint_ == rhs.waypoint_);
   equal &= (manipulator_info_ == rhs.manipulator_info_);
-  equal &= (profile_ == rhs.profile_);  // NO LINT
+  equal &= (profile_ == rhs.profile_);            // NO LINT
+  equal &= (path_profile_ == rhs.path_profile_);  // NO LINT
   return equal;
 }
 
@@ -109,6 +166,7 @@ void MoveInstruction::serialize(Archive& ar, const unsigned int /*version*/)
   ar& boost::serialization::make_nvp("move_type", move_type_);
   ar& boost::serialization::make_nvp("description", description_);
   ar& boost::serialization::make_nvp("profile", profile_);
+  ar& boost::serialization::make_nvp("path_profile", path_profile_);
   ar& boost::serialization::make_nvp("waypoint", waypoint_);
   ar& boost::serialization::make_nvp("manipulator_info", manipulator_info_);
 }

tesseract_command_language/src/plan_instruction.cpp

@@ -42,6 +42,21 @@ PlanInstruction::PlanInstruction(Waypoint waypoint,
   , waypoint_(std::move(waypoint))
   , profile_(std::move(profile))
   , manipulator_info_(std::move(manipulator_info))
+{
+  if (plan_type_ == PlanInstructionType::LINEAR || plan_type_ == PlanInstructionType::CIRCULAR)
+    path_profile_ = profile_;
+}
+
+PlanInstruction::PlanInstruction(Waypoint waypoint,
+                                 PlanInstructionType type,
+                                 std::string profile,
+                                 std::string path_profile,
+                                 ManipulatorInfo manipulator_info)
+  : plan_type_(type)
+  , waypoint_(std::move(waypoint))
+  , profile_(std::move(profile))
+  , path_profile_(std::move(path_profile))
+  , manipulator_info_(std::move(manipulator_info))
 {
 }
 
@@ -59,6 +74,9 @@ void PlanInstruction::setProfile(const std::string& profile)
 }
 const std::string& PlanInstruction::getProfile() const { return profile_; }
 
+void PlanInstruction::setPathProfile(const std::string& profile) { path_profile_ = profile; }
+const std::string& PlanInstruction::getPathProfile() const { return path_profile_; }
+
 const std::string& PlanInstruction::getDescription() const { return description_; }
 
 void PlanInstruction::setDescription(const std::string& description) { description_ = description; }
@@ -88,7 +106,8 @@ bool PlanInstruction::operator==(const PlanInstruction& rhs) const
   equal &= (static_cast<int>(plan_type_) == static_cast<int>(rhs.plan_type_));
   equal &= (waypoint_ == rhs.waypoint_);
   equal &= (manipulator_info_ == rhs.manipulator_info_);
-  equal &= (profile_ == rhs.profile_);  // NO LINT
+  equal &= (profile_ == rhs.profile_);            // NO LINT
+  equal &= (path_profile_ == rhs.path_profile_);  // NO LINT
   return equal;
 }
 
@@ -100,6 +119,7 @@ void PlanInstruction::serialize(Archive& ar, const unsigned int /*version*/)
   ar& boost::serialization::make_nvp("plan_type", plan_type_);
   ar& boost::serialization::make_nvp("description", description_);
   ar& boost::serialization::make_nvp("profile", profile_);
+  ar& boost::serialization::make_nvp("path_profile", path_profile_);
   ar& boost::serialization::make_nvp("waypoint", waypoint_);
   ar& boost::serialization::make_nvp("manipulator_info", manipulator_info_);
 }

tesseract_motion_planners/core/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h

@@ -55,7 +55,9 @@ class SimplePlannerFixedSizeAssignPlanProfile : public SimplePlannerPlanProfile
   SimplePlannerFixedSizeAssignPlanProfile(int freespace_steps = 10, int linear_steps = 10);
 
   CompositeInstruction generate(const PlanInstruction& prev_instruction,
+                                const MoveInstruction& prev_seed,
                                 const PlanInstruction& base_instruction,
+                                const Instruction& next_instruction,
                                 const PlannerRequest& request,
                                 const ManipulatorInfo& global_manip_info) const override;
 

tesseract_motion_planners/core/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h

@@ -54,7 +54,9 @@ class SimplePlannerFixedSizePlanProfile : public SimplePlannerPlanProfile
   SimplePlannerFixedSizePlanProfile(int freespace_steps = 10, int linear_steps = 10);
 
   CompositeInstruction generate(const PlanInstruction& prev_instruction,
+                                const MoveInstruction& prev_seed,
                                 const PlanInstruction& base_instruction,
+                                const Instruction& next_instruction,
                                 const PlannerRequest& request,
                                 const ManipulatorInfo& global_manip_info) const override;
 

tesseract_motion_planners/core/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h

@@ -63,7 +63,9 @@ class SimplePlannerLVSNoIKPlanProfile : public SimplePlannerPlanProfile
                                   int max_steps = std::numeric_limits<int>::max());
 
   CompositeInstruction generate(const PlanInstruction& prev_instruction,
+                                const MoveInstruction& prev_seed,
                                 const PlanInstruction& base_instruction,
+                                const Instruction& next_instruction,
                                 const PlannerRequest& request,
                                 const ManipulatorInfo& global_manip_info) const override;
 

tesseract_motion_planners/core/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h

@@ -61,7 +61,9 @@ class SimplePlannerLVSPlanProfile : public SimplePlannerPlanProfile
                               int min_steps = 1);
 
   CompositeInstruction generate(const PlanInstruction& prev_instruction,
+                                const MoveInstruction& prev_seed,
                                 const PlanInstruction& base_instruction,
+                                const Instruction& next_instruction,
                                 const PlannerRequest& request,
                                 const ManipulatorInfo& global_manip_info) const override;
 

tesseract_motion_planners/core/include/tesseract_motion_planners/simple/profile/simple_planner_profile.h

@@ -67,10 +67,33 @@ class SimplePlannerPlanProfile
    * @param global_manip_info The global manipulator information
    * @return A composite instruction representing the seed for the base_instruction
    */
+  virtual DEPRECATED() CompositeInstruction generate(const PlanInstruction& /*prev_instruction*/,
+                                                     const PlanInstruction& /*base_instruction*/,
+                                                     const PlannerRequest& /*request*/,
+                                                     const ManipulatorInfo& /*global_manip_info*/) const
+  {
+    throw std::runtime_error("SimplePlannerPlanProfile, this must be implemented in the derived class");
+  }
+
+  /**
+   * @brief Generate a seed for the provided base_instruction
+   * @param prev_instruction The previous instruction
+   * @param prev_seed The previous seed
+   * @param base_instruction The base/current instruction to generate the seed for
+   * @param next_instruction The next instruction. This will be a null instruction for the final instruction
+   * @param request The planning request
+   * @param global_manip_info The global manipulator information
+   * @return A composite instruction representing the seed for the base_instruction
+   */
   virtual CompositeInstruction generate(const PlanInstruction& prev_instruction,
+                                        const MoveInstruction& /*prev_seed*/,
                                         const PlanInstruction& base_instruction,
+                                        const Instruction& /*next_instruction*/,
                                         const PlannerRequest& request,
-                                        const ManipulatorInfo& global_manip_info) const = 0;
+                                        const ManipulatorInfo& global_manip_info) const
+  {
+    return generate(prev_instruction, base_instruction, request, global_manip_info);
+  }
 };
 
 class SimplePlannerCompositeProfile

tesseract_motion_planners/core/include/tesseract_motion_planners/simple/profile/simple_planner_utils.h

@@ -110,13 +110,6 @@ struct KinematicGroupInstructionInfo
   const Eigen::VectorXd& extractJointPosition() const;
 };
 
-/**
- * @brief Get the associated move instruction type for the give plan instruction type
- * @param base_instruction The plan instruction
- * @returnThe associated move instruction type for the give plan instruction type
- */
-MoveInstructionType getMoveInstructionType(const PlanInstruction& base_instruction);
-
 /**
  * @brief This takes the provided seed state for the base_instruction and create a corresponding composite instruction
  * @param joint_names The joint names associated with the states

tesseract_motion_planners/core/include/tesseract_motion_planners/simple/simple_motion_planner.h

@@ -97,6 +97,7 @@ class SimpleMotionPlanner : public MotionPlanner
 
   CompositeInstruction processCompositeInstruction(const CompositeInstruction& instructions,
                                                    PlanInstruction& prev_instruction,
+                                                   MoveInstruction& prev_seed,
                                                    const PlannerRequest& request) const;
 };
 

tesseract_motion_planners/core/src/simple/profile/simple_planner_fixed_size_assign_plan_profile.cpp

@@ -35,7 +35,9 @@ SimplePlannerFixedSizeAssignPlanProfile::SimplePlannerFixedSizeAssignPlanProfile
 }
 
 CompositeInstruction SimplePlannerFixedSizeAssignPlanProfile::generate(const PlanInstruction& prev_instruction,
+                                                                       const MoveInstruction& /*prev_seed*/,
                                                                        const PlanInstruction& base_instruction,
+                                                                       const Instruction& /*next_instruction*/,
                                                                        const PlannerRequest& request,
                                                                        const ManipulatorInfo& global_manip_info) const
 {

tesseract_motion_planners/core/src/simple/profile/simple_planner_fixed_size_plan_profile.cpp

@@ -35,7 +35,9 @@ SimplePlannerFixedSizePlanProfile::SimplePlannerFixedSizePlanProfile(int freespa
 }
 
 CompositeInstruction SimplePlannerFixedSizePlanProfile::generate(const PlanInstruction& prev_instruction,
+                                                                 const MoveInstruction& /*prev_seed*/,
                                                                  const PlanInstruction& base_instruction,
+                                                                 const Instruction& /*next_instruction*/,
                                                                  const PlannerRequest& request,
                                                                  const ManipulatorInfo& global_manip_info) const
 {