Fix tolerance size

rjoomen · rjoomen · commit 2297b9e98da9 · 2024-12-03T08:18:41.000+01:00
diff --git a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp
@@ -62,7 +62,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)
   {
@@ -71,7 +71,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
`@@ -62,7 +62,7 @@ std::shared_ptr<trajopt::TermInfo> createCartesianWaypointTermInfo(int index,`
`62`	`62`
`63`	`63`	`if (lower_tolerance.size() == 1)`
`64`	`64`	`{`
`65`		`- pose_info->lower_tolerance = Eigen::Vector3d::Constant(lower_tolerance(0));`
	`65`	`+ pose_info->lower_tolerance = Eigen::VectorXd::Constant(6, lower_tolerance(0));`
`66`	`66`	`}`
`67`	`67`	`else if (lower_tolerance.size() == 6)`
`68`	`68`	`{`
`@@ -71,7 +71,7 @@ std::shared_ptr<trajopt::TermInfo> createCartesianWaypointTermInfo(int index,`
`71`	`71`
`72`	`72`	`if (upper_tolerance.size() == 1)`
`73`	`73`	`{`
`74`		`- pose_info->upper_tolerance = Eigen::Vector3d::Constant(upper_tolerance(0));`
	`74`	`+ pose_info->upper_tolerance = Eigen::VectorXd::Constant(6, upper_tolerance(0));`
`75`	`75`	`}`
`76`	`76`	`else if (upper_tolerance.size() == 6)`
`77`	`77`	`{`