Fixed bug in linear approach/departure tool path modifiers (#222)

ros-industrial · Mar 6, 2024 · d3453e5 · d3453e5
1 parent db63b29
commit d3453e5
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 8 deletions.
diff --git a/noether_tpp/src/tool_path_modifiers/linear_approach_modifier.cpp b/noether_tpp/src/tool_path_modifiers/linear_approach_modifier.cpp
@@ -34,17 +34,17 @@ ToolPaths LinearApproachModifier::modify(ToolPaths tool_paths) const
   {
     for (ToolPathSegment& segment : tool_path)
     {
-      Eigen::Isometry3d offset_point = segment.front() * Eigen::Translation3d(offset_);
       ToolPathSegment new_segment;
       for (int i = 0; i < n_points_; i++)
       {
-        Eigen::Isometry3d pt;
-        pt = offset_point * Eigen::Translation3d(-(offset_ / (n_points_)*i));
+        Eigen::Isometry3d pt =
+            segment.front() *
+            Eigen::Translation3d(offset_ * (static_cast<double>(i + 1) / static_cast<double>(n_points_)));
         pt.linear() = segment.front().linear();
         new_segment.push_back(pt);
       }
 
-      segment.insert(segment.begin(), new_segment.begin(), new_segment.end());
+      segment.insert(segment.begin(), new_segment.rbegin(), new_segment.rend());
     }
   }
 

diff --git a/noether_tpp/src/tool_path_modifiers/linear_departure_modifier.cpp b/noether_tpp/src/tool_path_modifiers/linear_departure_modifier.cpp
@@ -11,15 +11,16 @@ ToolPaths LinearDepartureModifier::modify(ToolPaths tool_paths) const
       Eigen::Isometry3d offset_point = segment.back() * Eigen::Translation3d(offset_);
       ToolPathSegment new_segment;
 
-      for (int i = 0; i <= n_points_; i++)
+      for (int i = 0; i < n_points_; i++)
       {
-        Eigen::Isometry3d pt;
-        pt = offset_point * Eigen::Translation3d(-offset_ + (offset_ / (n_points_)) * i);
+        Eigen::Isometry3d pt =
+            segment.back() *
+            Eigen::Translation3d(offset_ * (static_cast<double>(i + 1) / static_cast<double>(n_points_)));
         pt.linear() = segment.back().linear();
         new_segment.push_back(pt);
       }
 
-      segment.insert(segment.end(), new_segment.rbegin(), new_segment.rend());
+      segment.insert(segment.end(), new_segment.begin(), new_segment.end());
     }
   }