Add check that costs/cnts are pushed to correct term info

tesseract-robotics · Dec 12, 2018 · 17849aa · 17849aa
1 parent 832d1c6
commit 17849aa
Showing 1 changed file with 7 additions and 3 deletions.
diff --git a/trajopt/src/problem_description.cpp b/trajopt/src/problem_description.cpp
@@ -390,6 +390,8 @@ TrajOptProbPtr ConstructProblem(const ProblemConstructionInfo& pci)
   // Check that all costs and constraints support the types that are specified in pci
   for (TermInfoPtr cost : pci.cost_infos)
   {
+    if (cost->term_type & TT_CNT)
+        ROS_WARN("%s is listed as a type TT_CNT but was added to cost_infos", (cost->name).c_str());
     if (!(cost->GetSupportedTypes() & TT_COST))
       PRINT_AND_THROW(boost::format("%s is only a constraint, but you listed it as a cost") % cost->name);
     if (cost->term_type & TT_USE_TIME)
@@ -401,6 +403,8 @@ TrajOptProbPtr ConstructProblem(const ProblemConstructionInfo& pci)
   }
   for (TermInfoPtr cnt : pci.cnt_infos)
   {
+    if (cnt->term_type & TT_COST)
+       ROS_WARN("%s is listed as a type TT_COST but was added to cnt_infos", (cnt->name).c_str());
     if (!(cnt->GetSupportedTypes() & TT_CNT))
       PRINT_AND_THROW(boost::format("%s is only a cost, but you listed it as a constraint") % cnt->name);
     if (cnt->term_type & TT_USE_TIME)
@@ -900,7 +904,7 @@ void JointVelTermInfo::hatch(TrajOptProb& prob)
 
   // Check if parameters are the correct size.
   checkParameterSize(coeffs, n_dof, "JointVelTermInfo coeffs", true);
-  checkParameterSize(targets, n_dof, "JointVelTermInfo upper_tols", true);
+  checkParameterSize(targets, n_dof, "JointVelTermInfo targets", true);
   checkParameterSize(upper_tols, n_dof, "JointVelTermInfo upper_tols", true);
   checkParameterSize(lower_tols, n_dof, "JointVelTermInfo lower_tols", true);
 
@@ -1027,7 +1031,7 @@ void JointAccTermInfo::hatch(TrajOptProb& prob)
 
   // Check if parameters are the correct size.
   checkParameterSize(coeffs, n_dof, "JointAccTermInfo coeffs", true);
-  checkParameterSize(targets, n_dof, "JointAccTermInfo upper_tols", true);
+  checkParameterSize(targets, n_dof, "JointAccTermInfo targets", true);
   checkParameterSize(upper_tols, n_dof, "JointAccTermInfo upper_tols", true);
   checkParameterSize(lower_tols, n_dof, "JointAccTermInfo lower_tols", true);
 
@@ -1155,7 +1159,7 @@ void JointJerkTermInfo::hatch(TrajOptProb& prob)
 
   // Check if parameters are the correct size.
   checkParameterSize(coeffs, n_dof, "JointJerkTermInfo coeffs", true);
-  checkParameterSize(targets, n_dof, "JointJerkTermInfo upper_tols", true);
+  checkParameterSize(targets, n_dof, "JointJerkTermInfo targets", true);
   checkParameterSize(upper_tols, n_dof, "JointJerkTermInfo upper_tols", true);
   checkParameterSize(lower_tols, n_dof, "JointJerkTermInfo lower_tols", true);