Fix issue with tolerance size

tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h

@@ -55,15 +55,18 @@ struct TrajOptCartesianWaypointConfig
    * This is useful if you want to have a smaller tolerance for the cost than the constraint.*/
   bool use_tolerance_override{ false };
 
-  /** @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::Matrix<double, 6, 1> lower_tolerance{ Eigen::VectorXd::Zero(6) };
-  /** @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::Matrix<double, 6, 1> upper_tolerance{ Eigen::VectorXd::Zero(6) };
-
-  /** @brief coefficients corresponsing to dx, dy, dz, rx, ry, rz*/
-  Eigen::Matrix<double, 6, 1> coeff{ Eigen::VectorXd::Constant(6, 5) };
+  /** @brief Distance below waypoint that is allowed. Should be size = 6 or 1, if size = 1 the value is replicated.
+   * 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{ Eigen::VectorXd::Zero(6) };
+  /** @brief Distance above waypoint that is allowed. Should be size = 6 or 1, if size = 1 the value is replicated.
+   * 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{ Eigen::VectorXd::Zero(6) };
+
+  /** @brief coefficients corresponsing to dx, dy, dz, rx, ry, rz.
+   * Should be size = 6 or 1, if size = 1 the value is replicated. */
+  Eigen::VectorXd coeff{ Eigen::VectorXd::Constant(6, 5) };
 
 private:
   friend class boost::serialization::access;

tesseract_motion_planners/trajopt/src/trajopt_utils.cpp

@@ -97,9 +97,45 @@ std::shared_ptr<trajopt::TermInfo> createCartesianWaypointTermInfo(int index,
     pose_info->pos_coeffs = coeffs.head<3>();
     pose_info->rot_coeffs = coeffs.tail<3>();
   }
+  else
+  {
+    throw std::runtime_error("Invalid coeffs size for Cartesian waypoint term info. Expected 1 or 6, got " +
+                             std::to_string(coeffs.size()));
+  }
 
-  pose_info->lower_tolerance = lower_tolerance;
-  pose_info->upper_tolerance = upper_tolerance;
+  if (lower_tolerance.size() > 0)
+  {
+    if (lower_tolerance.size() == 1)
+    {
+      pose_info->lower_tolerance = Eigen::VectorXd::Constant(6, lower_tolerance(0));
+    }
+    else if (lower_tolerance.size() == 6)
+    {
+      pose_info->lower_tolerance = lower_tolerance;
+    }
+    else
+    {
+      throw std::runtime_error("Invalid lower tolerance size for Cartesian waypoint term info. Expected 1 or 6, got " +
+                               std::to_string(lower_tolerance.size()));
+    }
+  }
+
+  if (upper_tolerance.size() > 0)
+  {
+    if (upper_tolerance.size() == 1)
+    {
+      pose_info->upper_tolerance = Eigen::VectorXd::Constant(6, upper_tolerance(0));
+    }
+    else if (upper_tolerance.size() == 6)
+    {
+      pose_info->upper_tolerance = upper_tolerance;
+    }
+    else
+    {
+      throw std::runtime_error("Invalid upper tolerance size for Cartesian waypoint term info. Expected 1 or 6, got " +
+                               std::to_string(upper_tolerance.size()));
+    }
+  }
 
   return pose_info;
 }
@@ -135,6 +171,11 @@ std::shared_ptr<trajopt::TermInfo> createDynamicCartesianWaypointTermInfo(int in
     pose->pos_coeffs = coeffs.head<3>();
     pose->rot_coeffs = coeffs.tail<3>();
   }
+  else
+  {
+    throw std::runtime_error("Invalid coeffs size for Cartesian waypoint term info. Expected 1 or 6, got " +
+                             std::to_string(coeffs.size()));
+  }
 
   pose->lower_tolerance = lower_tolerance;
   pose->upper_tolerance = upper_tolerance;
@@ -172,9 +213,18 @@ std::shared_ptr<trajopt::TermInfo> createJointWaypointTermInfo(const Eigen::Vect
 {
   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());
+  }
+  else
+  {
+    throw std::runtime_error("Invalid coeffs size for joint waypoint term info. Expected 1 or " +
+                             std::to_string(j_wp.size()) + ", got " + std::to_string(coeffs.size()));
+  }
 
   joint_info->targets = std::vector<double>(j_wp.data(), j_wp.data() + j_wp.rows() * j_wp.cols());
   joint_info->first_step = index;
@@ -194,9 +244,18 @@ std::shared_ptr<trajopt::TermInfo> createTolerancedJointWaypointTermInfo(const E
 {
   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());
+  }
+  else
+  {
+    throw std::runtime_error("Invalid coeffs size for joint waypoint term info. Expected 1 or " +
+                             std::to_string(j_wp.size()) + ", got " + std::to_string(coeffs.size()));
+  }
 
   joint_info->targets = std::vector<double>(j_wp.data(), j_wp.data() + j_wp.rows() * j_wp.cols());
   joint_info->lower_tols =