diff --git a/noether_gui/CMakeLists.txt b/noether_gui/CMakeLists.txt index 3ccfeda1..2473ac47 100644 --- a/noether_gui/CMakeLists.txt +++ b/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 diff --git a/noether_gui/include/noether_gui/widgets/tool_path_modifiers/biased_tool_drag_orientation_modifier_widget.h b/noether_gui/include/noether_gui/widgets/tool_path_modifiers/biased_tool_drag_orientation_modifier_widget.h new file mode 100644 index 00000000..84df326d --- /dev/null +++ b/noether_gui/include/noether_gui/widgets/tool_path_modifiers/biased_tool_drag_orientation_modifier_widget.h @@ -0,0 +1,26 @@ +#pragma once + +#include + +#include + +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 diff --git a/noether_gui/src/plugins.cpp b/noether_gui/src/plugins.cpp index cf132fd3..62681cc5 100644 --- a/noether_gui/src/plugins.cpp +++ b/noether_gui/src/plugins.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -96,6 +97,9 @@ using MovingAverageOrientationSmoothingModifierWidgetPlugin = using ToolDragOrientationToolPathModifierWidgetPlugin = WidgetPluginImpl; +using BiasedToolDragOrientationToolPathModifierWidgetPlugin = + WidgetPluginImpl; + using CircularLeadInToolPathModifierWidgetPlugin = WidgetPluginImpl; @@ -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) diff --git a/noether_gui/src/widgets/tool_path_modifiers/biased_tool_drag_orientation_modifier_widget.cpp b/noether_gui/src/widgets/tool_path_modifiers/biased_tool_drag_orientation_modifier_widget.cpp new file mode 100644 index 00000000..89077218 --- /dev/null +++ b/noether_gui/src/widgets/tool_path_modifiers/biased_tool_drag_orientation_modifier_widget.cpp @@ -0,0 +1,56 @@ +#include +#include + +#include +#include +#include +#include + +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(angle_offset_->value() * M_PI / 180.0, + tool_radius_->value()); +} + +void BiasedToolDragOrientationToolPathModifierWidget::configure(const YAML::Node& config) +{ + angle_offset_->setValue(getEntry(config, ANGLE_OFFSET_KEY)); + tool_radius_->setValue(getEntry(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 diff --git a/noether_tpp/CMakeLists.txt b/noether_tpp/CMakeLists.txt index 4198b94a..56ac63a7 100644 --- a/noether_tpp/CMakeLists.txt +++ b/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 diff --git a/noether_tpp/include/noether_tpp/tool_path_modifiers/biased_tool_drag_orientation_modifier.h b/noether_tpp/include/noether_tpp/tool_path_modifiers/biased_tool_drag_orientation_modifier.h new file mode 100644 index 00000000..ed493ea6 --- /dev/null +++ b/noether_tpp/include/noether_tpp/tool_path_modifiers/biased_tool_drag_orientation_modifier.h @@ -0,0 +1,22 @@ +#pragma once + +#include + +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 diff --git a/noether_tpp/src/tool_path_modifiers/biased_tool_drag_orientation_modifier.cpp b/noether_tpp/src/tool_path_modifiers/biased_tool_drag_orientation_modifier.cpp new file mode 100644 index 00000000..c4927e3a --- /dev/null +++ b/noether_tpp/src/tool_path_modifiers/biased_tool_drag_orientation_modifier.cpp @@ -0,0 +1,36 @@ +#include +#include + +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 diff --git a/noether_tpp/test/tool_path_modifier_utest.cpp b/noether_tpp/test/tool_path_modifier_utest.cpp index 681a685b..2050b07a 100644 --- a/noether_tpp/test/tool_path_modifier_utest.cpp +++ b/noether_tpp/test/tool_path_modifier_utest.cpp @@ -3,6 +3,7 @@ #include // Implementations +#include #include #include #include @@ -242,7 +243,8 @@ TEST_P(OneTimeToolPathModifierTestFixture, TestOperation) // Create a vector of implementations for the modifiers std::vector> createModifiers() { - return { std::make_shared(M_PI / 2.0, 0.1, 5), + return { std::make_shared(10 * M_PI / 180.0, 0.025), + std::make_shared(M_PI / 2.0, 0.1, 5), std::make_shared(M_PI / 2.0, 0.1, 5), std::make_shared(), std::make_shared(),