Fix issue with tolerance size

Author: rjoomen

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

@@ -53,15 +53,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) };
 
 protected:
   friend class boost::serialization::access;

tesseract_motion_planners/trajopt/src/trajopt_utils.cpp

@@ -98,8 +98,23 @@ std::shared_ptr<trajopt::TermInfo> createCartesianWaypointTermInfo(int index,
     pose_info->rot_coeffs = coeffs.tail<3>();
   }
 
-  pose_info->lower_tolerance = lower_tolerance;
-  pose_info->upper_tolerance = upper_tolerance;
+  if (lower_tolerance.size() == 1)
+  {
+    pose_info->lower_tolerance = Eigen::Vector3d::Constant(lower_tolerance(0));
+  }
+  else if (lower_tolerance.size() == 6)
+  {
+    pose_info->lower_tolerance = lower_tolerance;
+  }
+
+  if (upper_tolerance.size() == 1)
+  {
+    pose_info->upper_tolerance = Eigen::Vector3d::Constant(upper_tolerance(0));
+  }
+  else if (upper_tolerance.size() == 6)
+  {
+    pose_info->upper_tolerance = upper_tolerance;
+  }
 
   return pose_info;
 }