Added biased tool drag toolpath modifier (#279)

* Added biased tool drag toolpath modifier

* Added toolpath modifer to the unit test

noether_gui/CMakeLists.txt

@@ -57,6 +57,7 @@ add_library(${PROJECT_NAME} SHARED
   src/widgets/tool_path_modifiers/linear_approach_modifier_widget.cpp
   src/widgets/tool_path_modifiers/linear_departure_modifier_widget.cpp
   src/widgets/tool_path_modifiers/tool_drag_orientation_modifier_widget.cpp
+  src/widgets/tool_path_modifiers/biased_tool_drag_orientation_modifier_widget.cpp
   src/widgets/tool_path_modifiers/uniform_orientation_modifier_widget.cpp
   src/widgets/tool_path_modifiers/offset_modifier_widget.cpp
   src/widgets/tool_path_modifiers/uniform_spacing_spline_modifier_widget.cpp

noether_gui/include/noether_gui/widgets/tool_path_modifiers/biased_tool_drag_orientation_modifier_widget.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include <noether_gui/widgets.h>
+
+#include <noether_tpp/core/tool_path_modifier.h>
+
+class QDoubleSpinBox;
+
+namespace noether
+{
+class BiasedToolDragOrientationToolPathModifierWidget : public ToolPathModifierWidget
+{
+public:
+  BiasedToolDragOrientationToolPathModifierWidget(QWidget* parent = nullptr);
+
+  ToolPathModifier::ConstPtr create() const override;
+
+  void configure(const YAML::Node&) override;
+  void save(YAML::Node&) const override;
+
+private:
+  QDoubleSpinBox* angle_offset_;
+  QDoubleSpinBox* tool_radius_;
+};
+
+}  // namespace noether

noether_gui/src/plugins.cpp

@@ -25,6 +25,7 @@
 #include <noether_gui/widgets/tool_path_modifiers/snake_organization_modifier_widget.h>
 #include <noether_gui/widgets/tool_path_modifiers/standard_edge_paths_organization_modifier_widget.h>
 #include <noether_gui/widgets/tool_path_modifiers/tool_drag_orientation_modifier_widget.h>
+#include <noether_gui/widgets/tool_path_modifiers/biased_tool_drag_orientation_modifier_widget.h>
 #include <noether_gui/widgets/tool_path_modifiers/uniform_orientation_modifier_widget.h>
 #include <noether_gui/widgets/tool_path_modifiers/linear_approach_modifier_widget.h>
 #include <noether_gui/widgets/tool_path_modifiers/linear_departure_modifier_widget.h>
@@ -96,6 +97,9 @@ using MovingAverageOrientationSmoothingModifierWidgetPlugin =
 using ToolDragOrientationToolPathModifierWidgetPlugin =
     WidgetPluginImpl<ToolDragOrientationToolPathModifierWidget, ToolPathModifierWidget>;
 
+using BiasedToolDragOrientationToolPathModifierWidgetPlugin =
+    WidgetPluginImpl<BiasedToolDragOrientationToolPathModifierWidget, ToolPathModifierWidget>;
+
 using CircularLeadInToolPathModifierWidgetPlugin =
     WidgetPluginImpl<CircularLeadInToolPathModifierWidget, ToolPathModifierWidget>;
 
@@ -192,6 +196,8 @@ EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::MovingAverageOrientationSmoothi
                                         MovingAverageOrientationSmoothingModifier)
 EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::ToolDragOrientationToolPathModifierWidgetPlugin,
                                         ToolDragOrientationToolPathModifier)
+EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::BiasedToolDragOrientationToolPathModifierWidgetPlugin,
+                                        BiasedToolDragOrientationToolPathModifier)
 EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::CircularLeadInToolPathModifierWidgetPlugin, CircularLeadInModifier)
 EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::CircularLeadOutToolPathModifierWidgetPlugin, CircularLeadOutModifier)
 EXPORT_TOOL_PATH_MODIFIER_WIDGET_PLUGIN(noether::LinearApproachToolPathModifierWidgetPlugin, LinearApproachModifier)

noether_gui/src/widgets/tool_path_modifiers/biased_tool_drag_orientation_modifier_widget.cpp

@@ -0,0 +1,56 @@
+#include <noether_gui/widgets/tool_path_modifiers/biased_tool_drag_orientation_modifier_widget.h>
+#include <noether_gui/utils.h>
+
+#include <noether_tpp/tool_path_modifiers/biased_tool_drag_orientation_modifier.h>
+#include <QFormLayout>
+#include <QLabel>
+#include <QDoubleSpinBox>
+
+static const std::string ANGLE_OFFSET_KEY = "angle_offset";
+static const std::string TOOL_RADIUS_KEY = "tool_radius";
+
+namespace noether
+{
+BiasedToolDragOrientationToolPathModifierWidget::BiasedToolDragOrientationToolPathModifierWidget(QWidget* parent)
+  : ToolPathModifierWidget(parent)
+{
+  auto layout = new QFormLayout(this);
+
+  // Angle between the grinder and the media being ground
+  angle_offset_ = new QDoubleSpinBox(this);
+  angle_offset_->setMinimum(0.0);
+  angle_offset_->setSingleStep(1.0);
+  angle_offset_->setValue(30.0);
+  angle_offset_->setDecimals(3);
+  auto label = new QLabel("Angle offset (deg)", this);
+  label->setToolTip("The angle between the grinder and the media being ground");
+  layout->addRow(label, angle_offset_);
+
+  // Radius of the tool
+  tool_radius_ = new QDoubleSpinBox(this);
+  tool_radius_->setMinimum(0.0);
+  tool_radius_->setSingleStep(0.01);
+  tool_radius_->setValue(0.1);
+  tool_radius_->setDecimals(3);
+  layout->addRow(new QLabel("Tool radius (m)", this), tool_radius_);
+}
+
+ToolPathModifier::ConstPtr BiasedToolDragOrientationToolPathModifierWidget::create() const
+{
+  return std::make_unique<BiasedToolDragOrientationToolPathModifier>(angle_offset_->value() * M_PI / 180.0,
+                                                                     tool_radius_->value());
+}
+
+void BiasedToolDragOrientationToolPathModifierWidget::configure(const YAML::Node& config)
+{
+  angle_offset_->setValue(getEntry<double>(config, ANGLE_OFFSET_KEY));
+  tool_radius_->setValue(getEntry<double>(config, TOOL_RADIUS_KEY));
+}
+
+void BiasedToolDragOrientationToolPathModifierWidget::save(YAML::Node& config) const
+{
+  config[ANGLE_OFFSET_KEY] = angle_offset_->value();
+  config[TOOL_RADIUS_KEY] = tool_radius_->value();
+}
+
+}  // namespace noether

noether_tpp/CMakeLists.txt

@@ -35,6 +35,7 @@ add_library(${PROJECT_NAME} SHARED
   src/mesh_modifiers/normal_estimation_pcl.cpp
   src/mesh_modifiers/normals_from_mesh_faces_modifier.cpp
   # Tool Path Modifiers
+  src/tool_path_modifiers/biased_tool_drag_orientation_modifier.cpp
   src/tool_path_modifiers/circular_lead_in_modifier.cpp
   src/tool_path_modifiers/circular_lead_out_modifier.cpp
   src/tool_path_modifiers/compound_modifier.cpp

noether_tpp/include/noether_tpp/tool_path_modifiers/biased_tool_drag_orientation_modifier.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <noether_tpp/core/tool_path_modifier.h>
+
+namespace noether
+{
+/**
+ * @brief Transforms the waypoints to correspond with the edge of the ginding tool so that the edge
+ * of the tool is in contact with the media
+ */
+class BiasedToolDragOrientationToolPathModifier : public ToolPathModifier
+{
+public:
+  BiasedToolDragOrientationToolPathModifier(double angle_offset, double tool_radius);
+  ToolPaths modify(ToolPaths tool_paths) const override final;
+
+protected:
+  const double angle_offset_;
+  const double tool_radius_;
+};
+
+}  // namespace noether

noether_tpp/src/tool_path_modifiers/biased_tool_drag_orientation_modifier.cpp

@@ -0,0 +1,36 @@
+#include <noether_tpp/tool_path_modifiers/biased_tool_drag_orientation_modifier.h>
+#include <noether_tpp/utils.h>
+
+namespace noether
+{
+BiasedToolDragOrientationToolPathModifier::BiasedToolDragOrientationToolPathModifier(double angle_offset,
+                                                                                     double tool_radius)
+  : angle_offset_(angle_offset), tool_radius_(tool_radius)
+{
+}
+
+ToolPaths BiasedToolDragOrientationToolPathModifier::modify(ToolPaths tool_paths) const
+{
+  for (ToolPath& tool_path : tool_paths)
+  {
+    Eigen::Vector3d dir = estimateToolPathDirection(tool_path);
+    const Eigen::Isometry3d& first = tool_path.at(0).at(0);
+    const Eigen::Vector3d& y = first.matrix().col(1).head<3>();
+    const Eigen::Vector3d& z = first.matrix().col(2).head<3>();
+    // Sign is determined by whether the first point's y-axis is aligned with the nominal path y-axis (i.e., the cross
+    // of the waypoint's z-axis with the nominal path direction)
+    double sign = z.cross(dir).dot(y) > 0.0 ? 1.0 : -1.0;
+    for (ToolPathSegment& segment : tool_path)
+    {
+      for (Eigen::Isometry3d& waypoint : segment)
+      {
+        waypoint.rotate(Eigen::AngleAxisd(sign * -angle_offset_, Eigen::Vector3d::UnitY()))
+            .translate(Eigen::Vector3d(sign * tool_radius_, 0, 0));
+      }
+    }
+  }
+
+  return tool_paths;
+}
+
+}  // namespace noether

noether_tpp/test/tool_path_modifier_utest.cpp

@@ -3,6 +3,7 @@
 
 #include <noether_tpp/core/tool_path_modifier.h>
 // Implementations
+#include <noether_tpp/tool_path_modifiers/biased_tool_drag_orientation_modifier.h>
 #include <noether_tpp/tool_path_modifiers/circular_lead_in_modifier.h>
 #include <noether_tpp/tool_path_modifiers/circular_lead_out_modifier.h>
 #include <noether_tpp/tool_path_modifiers/concatenate_modifier.h>
@@ -242,7 +243,8 @@ TEST_P(OneTimeToolPathModifierTestFixture, TestOperation)
 // Create a vector of implementations for the modifiers
 std::vector<std::shared_ptr<const ToolPathModifier>> createModifiers()
 {
-  return { std::make_shared<CircularLeadInModifier>(M_PI / 2.0, 0.1, 5),
+  return { std::make_shared<BiasedToolDragOrientationToolPathModifier>(10 * M_PI / 180.0, 0.025),
+           std::make_shared<CircularLeadInModifier>(M_PI / 2.0, 0.1, 5),
            std::make_shared<CircularLeadOutModifier>(M_PI / 2.0, 0.1, 5),
            std::make_shared<ConcatenateModifier>(),
            std::make_shared<DirectionOfTravelOrientationModifier>(),