Add tolerances to joint/cartesian waypoints

mpowelson · mpowelson · commit 5c5509e2c107 · 2020-11-12T15:23:03.000-07:00
diff --git a/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h b/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h
@@ -257,6 +257,25 @@ class CartesianWaypoint
 
   /** @brief The Cartesian Waypoint */
   Eigen::Isometry3d waypoint;
+  /** @brief Distance below waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()) */
+  Eigen::VectorXd lower_tolerance;
+  /** @brief Distance above waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle())*/
+  Eigen::VectorXd upper_tolerance;
+
+  bool isToleranced() const
+  {
+    // Check if they are empty
+    if (lower_tolerance.size() == 0 || upper_tolerance.size() == 0)
+      return false;
+
+    // Check if they are close to 0
+    Eigen::VectorXd range = upper_tolerance - lower_tolerance;
+    double sum = range.sum();
+    if (sum < 1e-5)
+      return false;
+
+    return true;
+  }
 };
 
 }  // namespace tesseract_planning
diff --git a/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h b/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h
@@ -258,11 +258,30 @@ class JointWaypoint
   inline operator Eigen::Ref<Eigen::VectorXd>() { return Eigen::Ref<Eigen::VectorXd>(waypoint); }
 
   //////////////////////////////////
-  // Cartesian Waypoint Container //
+  // Joint Waypoint Container //
   //////////////////////////////////
 
   Eigen::VectorXd waypoint;
   std::vector<std::string> joint_names;
+  /** @brief Joint distance below waypoint that is allowed. Each element should be <= 0 */
+  Eigen::VectorXd lower_tolerance;
+  /** @brief Joint distance above waypoint that is allowed. Each element should be >= 0 */
+  Eigen::VectorXd upper_tolerance;
+
+  bool isToleranced() const
+  {
+    // Check if they are empty
+    if (lower_tolerance.size() == 0 || upper_tolerance.size() == 0)
+      return false;
+
+    // Check if they are close to 0
+    Eigen::VectorXd range = upper_tolerance - lower_tolerance;
+    double sum = range.sum();
+    if (sum < 1e-5)
+      return false;
+
+    return true;
+  }
 };
 }  // namespace tesseract_planning
 
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h
@@ -33,6 +33,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <Eigen/Geometry>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
+#include <tesseract_command_language/cartesian_waypoint.h>
+#include <tesseract_command_language/joint_waypoint.h>
 #include <tesseract_motion_planners/trajopt/profile/trajopt_profile.h>
 
 namespace tesseract_planning
@@ -72,14 +74,14 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile
       constraint_error_functions;
 
   void apply(trajopt::ProblemConstructionInfo& pci,
-             const Eigen::Isometry3d& cartesian_waypoint,
+             const CartesianWaypoint& cartesian_waypoint,
              const Instruction& parent_instruction,
              const ManipulatorInfo& manip_info,
              const std::vector<std::string>& active_links,
              int index) override;
 
   void apply(trajopt::ProblemConstructionInfo& pci,
-             const Eigen::VectorXd& joint_waypoint,
+             const JointWaypoint& joint_waypoint,
              const Instruction& parent_instruction,
              const ManipulatorInfo& manip_info,
              const std::vector<std::string>& active_links,
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h
@@ -34,6 +34,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_command_language/core/instruction.h>
+#include <tesseract_command_language/cartesian_waypoint.h>
+#include <tesseract_command_language/joint_waypoint.h>
 #include <tesseract_command_language/manipulator_info.h>
 
 namespace tesseract_planning
@@ -45,14 +47,14 @@ class TrajOptPlanProfile
   using ConstPtr = std::shared_ptr<const TrajOptPlanProfile>;
 
   virtual void apply(trajopt::ProblemConstructionInfo& pci,
-                     const Eigen::Isometry3d& cartesian_waypoint,
+                     const CartesianWaypoint& cartesian_waypoint,
                      const Instruction& parent_instruction,
                      const ManipulatorInfo& manip_info,
                      const std::vector<std::string>& active_links,
                      int index) = 0;
 
   virtual void apply(trajopt::ProblemConstructionInfo& pci,
-                     const Eigen::VectorXd& joint_waypoint,
+                     const JointWaypoint& joint_waypoint,
                      const Instruction& parent_instruction,
                      const ManipulatorInfo& manip_info,
                      const std::vector<std::string>& active_links,
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h
@@ -63,6 +63,14 @@ trajopt::TermInfo::Ptr createJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
                                                    const Eigen::VectorXd& coeffs,
                                                    trajopt::TermType type);
 
+trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
+                                                             const Eigen::VectorXd& lower_tol,
+                                                             const Eigen::VectorXd& upper_tol,
+                                                   int index,
+                                                   const Eigen::VectorXd& coeffs,
+                                                   trajopt::TermType type);
+
+
 trajopt::TermInfo::Ptr createCollisionTermInfo(
     int start_index,
     int end_index,
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp
@@ -126,8 +126,9 @@ trajopt::TrajOptProb::Ptr DefaultTrajoptProblemGenerator(const std::string& name
   }
   else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint))
   {
-    const Eigen::VectorXd& position = getJointPosition(start_waypoint);
-    start_plan_profile->apply(*pci, position, *start_instruction, composite_mi, active_links, index);
+    JointWaypoint position_wp;
+    position_wp.waypoint = getJointPosition(start_waypoint);
+    start_plan_profile->apply(*pci, position_wp, *start_instruction, composite_mi, active_links, index);
   }
   else
   {
@@ -213,7 +214,8 @@ trajopt::TrajOptProb::Ptr DefaultTrajoptProblemGenerator(const std::string& name
         }
         else if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
         {
-          const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
+          JointWaypoint cur_position;
+          cur_position.waypoint = getJointPosition(plan_instruction->getWaypoint());
           Eigen::Isometry3d cur_pose = Eigen::Isometry3d::Identity();
           if (!pci->kin->calcFwdKin(cur_pose, cur_position))
             throw std::runtime_error("TrajOptPlannerUniversalConfig: failed to solve forward kinematics!");
@@ -271,7 +273,8 @@ trajopt::TrajOptProb::Ptr DefaultTrajoptProblemGenerator(const std::string& name
       {
         if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
         {
-          const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());
+          JointWaypoint cur_position;
+          cur_position.waypoint = getJointPosition(plan_instruction->getWaypoint());
 
           // Add intermediate points with path costs and constraints
           for (std::size_t s = 0; s < seed_composite->size() - 1; ++s)
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/profile/trajopt_default_plan_profile.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/profile/trajopt_default_plan_profile.cpp
@@ -99,7 +99,7 @@ TrajOptDefaultPlanProfile::TrajOptDefaultPlanProfile(const tinyxml2::XMLElement&
   }
 }
 void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,
-                                      const Eigen::Isometry3d& cartesian_waypoint,
+                                      const CartesianWaypoint& cartesian_waypoint,
                                       const Instruction& parent_instruction,
                                       const ManipulatorInfo& manip_info,
                                       const std::vector<std::string>& active_links,
@@ -119,11 +119,17 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,
   auto it = std::find(active_links.begin(), active_links.end(), mi.working_frame);
   if (it != active_links.end())
   {
+    if (cartesian_waypoint.isToleranced())
+       CONSOLE_BRIDGE_logWarn("Toleranced cartesian waypoints are not supported in this version of TrajOpt.");
+
     ti = createDynamicCartesianWaypointTermInfo(
         cartesian_waypoint, index, mi.working_frame, tcp, cartesian_coeff, pci.kin->getTipLinkName(), term_type);
   }
   else
   {
+    if (cartesian_waypoint.isToleranced())
+       CONSOLE_BRIDGE_logWarn("Toleranced cartesian waypoints are not supported in this version of TrajOpt.");
+
     ti = createCartesianWaypointTermInfo(
         cartesian_waypoint, index, mi.working_frame, tcp, cartesian_coeff, pci.kin->getTipLinkName(), term_type);
   }
@@ -138,13 +144,17 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,
 }
 
 void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,
-                                      const Eigen::VectorXd& joint_waypoint,
+                                      const JointWaypoint& joint_waypoint,
                                       const Instruction& /*parent_instruction*/,
                                       const ManipulatorInfo& /*manip_info*/,
                                       const std::vector<std::string>& /*active_links*/,
                                       int index)
 {
-  auto ti = createJointWaypointTermInfo(joint_waypoint, index, joint_coeff, term_type);
+  trajopt::TermInfo::Ptr ti;
+  if (joint_waypoint.isToleranced())
+    ti = createTolerancedJointWaypointTermInfo(joint_waypoint, joint_waypoint.lower_tolerance, joint_waypoint.upper_tolerance, index, joint_coeff, term_type);
+  else
+    ti = createJointWaypointTermInfo(joint_waypoint, index, joint_coeff, term_type);
 
   if (term_type == trajopt::TermType::TT_CNT)
     pci.cnt_infos.push_back(ti);
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_utils.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_utils.cpp
@@ -138,6 +138,31 @@ trajopt::TermInfo::Ptr createJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
   return joint_info;
 }
 
+trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
+                                                             const Eigen::VectorXd& lower_tol,
+                                                             const Eigen::VectorXd& upper_tol,
+                                                   int index,
+                                                   const Eigen::VectorXd& coeffs,
+                                                   trajopt::TermType type)
+{
+  auto joint_info = std::make_shared<trajopt::JointPosTermInfo>();
+  if (coeffs.size() == 1)
+    joint_info->coeffs = std::vector<double>(static_cast<std::size_t>(j_wp.size()), coeffs(0));
+  else if (coeffs.size() == j_wp.size())
+    joint_info->coeffs = std::vector<double>(coeffs.data(), coeffs.data() + coeffs.rows() * coeffs.cols());
+
+  joint_info->targets = std::vector<double>(j_wp.data(), j_wp.data() + j_wp.rows() * j_wp.cols());
+  joint_info->lower_tols = std::vector<double>(lower_tol.data(), lower_tol.data() + lower_tol.rows() * lower_tol.cols());
+  joint_info->upper_tols = std::vector<double>(upper_tol.data(), upper_tol.data() + upper_tol.rows() * upper_tol.cols());
+  joint_info->first_step = index;
+  joint_info->last_step = index;
+  joint_info->name = "joint_waypoint_" + std::to_string(index);
+  joint_info->term_type = type;
+
+  return joint_info;
+}
+
+
 trajopt::TermInfo::Ptr createCollisionTermInfo(int start_index,
                                                int end_index,
                                                double collision_safety_margin,

Original file line number	Diff line number	Diff line change
`@@ -126,8 +126,9 @@ trajopt::TrajOptProb::Ptr DefaultTrajoptProblemGenerator(const std::string& name`
`126`	`126`	`}`
`127`	`127`	`else if (isJointWaypoint(start_waypoint) \|\| isStateWaypoint(start_waypoint))`
`128`	`128`	`{`
`129`		`- const Eigen::VectorXd& position = getJointPosition(start_waypoint);`
`130`		`- start_plan_profile->apply(pci, position, start_instruction, composite_mi, active_links, index);`
	`129`	`+ JointWaypoint position_wp;`
	`130`	`+ position_wp.waypoint = getJointPosition(start_waypoint);`
	`131`	`+ start_plan_profile->apply(pci, position_wp, start_instruction, composite_mi, active_links, index);`
`131`	`132`	`}`
`132`	`133`	`else`
`133`	`134`	`{`
`@@ -213,7 +214,8 @@ trajopt::TrajOptProb::Ptr DefaultTrajoptProblemGenerator(const std::string& name`
`213`	`214`	`}`
`214`	`215`	`else if (isJointWaypoint(plan_instruction->getWaypoint()) \|\| isStateWaypoint(plan_instruction->getWaypoint()))`
`215`	`216`	`{`
`216`		`- const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());`
	`217`	`+ JointWaypoint cur_position;`
	`218`	`+ cur_position.waypoint = getJointPosition(plan_instruction->getWaypoint());`
`217`	`219`	`Eigen::Isometry3d cur_pose = Eigen::Isometry3d::Identity();`
`218`	`220`	`if (!pci->kin->calcFwdKin(cur_pose, cur_position))`
`219`	`221`	`throw std::runtime_error("TrajOptPlannerUniversalConfig: failed to solve forward kinematics!");`
`@@ -271,7 +273,8 @@ trajopt::TrajOptProb::Ptr DefaultTrajoptProblemGenerator(const std::string& name`
`271`	`273`	`{`
`272`	`274`	`if (isJointWaypoint(plan_instruction->getWaypoint()) \|\| isStateWaypoint(plan_instruction->getWaypoint()))`
`273`	`275`	`{`
`274`		`- const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint());`
	`276`	`+ JointWaypoint cur_position;`
	`277`	`+ cur_position.waypoint = getJointPosition(plan_instruction->getWaypoint());`
`275`	`278`
`276`	`279`	`// Add intermediate points with path costs and constraints`
`277`	`280`	`for (std::size_t s = 0; s < seed_composite->size() - 1; ++s)`
Original file line number	Diff line number	Diff line change
`@@ -99,7 +99,7 @@ TrajOptDefaultPlanProfile::TrajOptDefaultPlanProfile(const tinyxml2::XMLElement&`
`99`	`99`	`}`
`100`	`100`	`}`
`101`	`101`	`void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,`
`102`		`- const Eigen::Isometry3d& cartesian_waypoint,`
	`102`	`+ const CartesianWaypoint& cartesian_waypoint,`
`103`	`103`	`const Instruction& parent_instruction,`
`104`	`104`	`const ManipulatorInfo& manip_info,`
`105`	`105`	`const std::vector<std::string>& active_links,`
`@@ -119,11 +119,17 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,`
`119`	`119`	`auto it = std::find(active_links.begin(), active_links.end(), mi.working_frame);`
`120`	`120`	`if (it != active_links.end())`
`121`	`121`	`{`
	`122`	`+ if (cartesian_waypoint.isToleranced())`
	`123`	`+ CONSOLE_BRIDGE_logWarn("Toleranced cartesian waypoints are not supported in this version of TrajOpt.");`
	`124`	`+`
`122`	`125`	`ti = createDynamicCartesianWaypointTermInfo(`
`123`	`126`	`cartesian_waypoint, index, mi.working_frame, tcp, cartesian_coeff, pci.kin->getTipLinkName(), term_type);`
`124`	`127`	`}`
`125`	`128`	`else`
`126`	`129`	`{`
	`130`	`+ if (cartesian_waypoint.isToleranced())`
	`131`	`+ CONSOLE_BRIDGE_logWarn("Toleranced cartesian waypoints are not supported in this version of TrajOpt.");`
	`132`	`+`
`127`	`133`	`ti = createCartesianWaypointTermInfo(`
`128`	`134`	`cartesian_waypoint, index, mi.working_frame, tcp, cartesian_coeff, pci.kin->getTipLinkName(), term_type);`
`129`	`135`	`}`
`@@ -138,13 +144,17 @@ void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,`
`138`	`144`	`}`
`139`	`145`
`140`	`146`	`void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci,`
`141`		`- const Eigen::VectorXd& joint_waypoint,`
	`147`	`+ const JointWaypoint& joint_waypoint,`
`142`	`148`	`const Instruction& /parent_instruction/,`
`143`	`149`	`const ManipulatorInfo& /manip_info/,`
`144`	`150`	`const std::vector<std::string>& /active_links/,`
`145`	`151`	`int index)`
`146`	`152`	`{`
`147`		`- auto ti = createJointWaypointTermInfo(joint_waypoint, index, joint_coeff, term_type);`
	`153`	`+ trajopt::TermInfo::Ptr ti;`
	`154`	`+ if (joint_waypoint.isToleranced())`
	`155`	`+ ti = createTolerancedJointWaypointTermInfo(joint_waypoint, joint_waypoint.lower_tolerance, joint_waypoint.upper_tolerance, index, joint_coeff, term_type);`
	`156`	`+ else`
	`157`	`+ ti = createJointWaypointTermInfo(joint_waypoint, index, joint_coeff, term_type);`
`148`	`158`
`149`	`159`	`if (term_type == trajopt::TermType::TT_CNT)`
`150`	`160`	`pci.cnt_infos.push_back(ti);`