Throw an exception when number of coeffs or tolerances is incorrect

Author: rjoomen

tesseract_motion_planners/trajopt/src/trajopt_utils.cpp

@@ -97,6 +97,11 @@ 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()));
+  }
 
   if (lower_tolerance.size() == 1)
   {
@@ -106,6 +111,11 @@ std::shared_ptr<trajopt::TermInfo> createCartesianWaypointTermInfo(int index,
   {
     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() == 1)
   {
@@ -115,6 +125,11 @@ std::shared_ptr<trajopt::TermInfo> createCartesianWaypointTermInfo(int index,
   {
     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;
 }
@@ -150,6 +165,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;
@@ -187,9 +207,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;
@@ -209,9 +238,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 =