Fix tolerance size

rjoomen · rjoomen · commit be2e05581e6f · 2025-03-26T11:51:50.000+01:00
diff --git a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp
@@ -100,7 +100,7 @@ std::shared_ptr<trajopt::TermInfo> createCartesianWaypointTermInfo(int index,
 
   if (lower_tolerance.size() == 1)
   {
-    pose_info->lower_tolerance = Eigen::Vector3d::Constant(lower_tolerance(0));
+    pose_info->lower_tolerance = Eigen::VectorXd::Constant(6, lower_tolerance(0));
   }
   else if (lower_tolerance.size() == 6)
   {
@@ -109,7 +109,7 @@ std::shared_ptr<trajopt::TermInfo> createCartesianWaypointTermInfo(int index,
 
   if (upper_tolerance.size() == 1)
   {
-    pose_info->upper_tolerance = Eigen::Vector3d::Constant(upper_tolerance(0));
+    pose_info->upper_tolerance = Eigen::VectorXd::Constant(6, upper_tolerance(0));
   }
   else if (upper_tolerance.size() == 6)
   {

Original file line number	Diff line number	Diff line change
`@@ -100,7 +100,7 @@ std::shared_ptr<trajopt::TermInfo> createCartesianWaypointTermInfo(int index,`
`100`	`100`
`101`	`101`	`if (lower_tolerance.size() == 1)`
`102`	`102`	`{`
`103`		`- pose_info->lower_tolerance = Eigen::Vector3d::Constant(lower_tolerance(0));`
	`103`	`+ pose_info->lower_tolerance = Eigen::VectorXd::Constant(6, lower_tolerance(0));`
`104`	`104`	`}`
`105`	`105`	`else if (lower_tolerance.size() == 6)`
`106`	`106`	`{`
`@@ -109,7 +109,7 @@ std::shared_ptr<trajopt::TermInfo> createCartesianWaypointTermInfo(int index,`
`109`	`109`
`110`	`110`	`if (upper_tolerance.size() == 1)`
`111`	`111`	`{`
`112`		`- pose_info->upper_tolerance = Eigen::Vector3d::Constant(upper_tolerance(0));`
	`112`	`+ pose_info->upper_tolerance = Eigen::VectorXd::Constant(6, upper_tolerance(0));`
`113`	`113`	`}`
`114`	`114`	`else if (upper_tolerance.size() == 6)`
`115`	`115`	`{`