584 task gripper controller merge into main we want juicy gripper inside of main#10
Open
ApatShe wants to merge 25 commits into
Open
584 task gripper controller merge into main we want juicy gripper inside of main#10ApatShe wants to merge 25 commits into
ApatShe wants to merge 25 commits into
Conversation
…lder as well as removed the previously added dependencies as consequence of making the controller module within the interface
… originally in the gripper_interface/
…n vortex_auv repo: vortex-auv/guidance/reference_filter_dp -Rather than following the recipe to construct the model after p. 337 EQ 12.11 and 12.12 in Thor I. Fossens book, dimensions and names have been changed to match the p.336 EQ 12.5 and 12.6. -Notice several TODO's for future work and consultation with Cyp & Jøg
…with entire system, needs actual servo states to simulate proper topic relationships
…4-task-gripper-controller
…im interface with functioning goal sends to ref filter playing out in sim environment
jorgenfj
requested changes
Apr 26, 2026
jorgenfj
left a comment
jorgenfj
left a comment
There was a problem hiding this comment.
The dp reference filter has been rewritten to have a separate cpp class to handle the action state execution. Up to you whether you want to implement that here as well, both options are fine.
Also should look over the files changed and just slap const qualifiers on variables where it is suited.
Comment on lines
+118
to
+127
| types::GripperState measured_state; | ||
| types::GripperState reference_state; | ||
|
|
||
| { | ||
| std::lock_guard<std::mutex> lock(state_mutex_); | ||
| measured_state.roll = roll_measured_; | ||
| measured_state.pinch = pinch_measured_; | ||
| reference_state.roll = roll_ref_; | ||
| reference_state.pinch = pinch_ref_; | ||
| } |
There was a problem hiding this comment.
These local state variables should preferably be a const.
Should look into this for complex const initialization using lambdas
Comment on lines
+46
to
+49
| reference_sub_ = this->create_subscription<vortex_msgs::msg::GripperState>( | ||
| gripper_state_topic, qos_sensor_data, | ||
| std::bind(&GripperReferenceFilterNode::reference_callback, this, | ||
| std::placeholders::_1)); |
There was a problem hiding this comment.
Consider using lambdas instead of std::bind to avoid using std::placeholders
Comment on lines
+59
to
+75
| action_server_ = rclcpp_action::create_server< | ||
| vortex_msgs::action::GripperReferenceFilterWaypoint>( | ||
|
|
||
| this, | ||
|
|
||
| action_server_name, | ||
|
|
||
| std::bind(&GripperReferenceFilterNode::handle_goal, this, | ||
| std::placeholders::_1, std::placeholders::_2), | ||
|
|
||
| std::bind(&GripperReferenceFilterNode::handle_cancel, this, | ||
| std::placeholders::_1), | ||
|
|
||
| std::bind(&GripperReferenceFilterNode::handle_accepted, this, | ||
| std::placeholders::_1), | ||
|
|
||
| rcl_action_server_get_default_options(), cb_group_); |
There was a problem hiding this comment.
What is this line spacing? I need a vertical monitor to read this...
Comment on lines
+98
to
+103
| rclcpp_action::GoalResponse GripperReferenceFilterNode::handle_goal( | ||
| const rclcpp_action::GoalUUID& uuid, | ||
| std::shared_ptr<const vortex_msgs::action::GripperReferenceFilterWaypoint::Goal> | ||
| goal) { | ||
| (void)uuid; | ||
| (void)goal; |
There was a problem hiding this comment.
For unused function args a more explicit approach is to comment out the args themselvs instead of using (void).
const rclcpp_action::GoalUUID& /*uuid*/,
std::shared_ptr<const vortex_msgs::action::GripperReferenceFilterWaypoint::Goal>
/*goal*/
Comment on lines
+125
to
+130
| void GripperReferenceFilterNode::handle_accepted( | ||
| const std::shared_ptr<rclcpp_action::ServerGoalHandle< | ||
| vortex_msgs::action::GripperReferenceFilterWaypoint>> goal_handle) { | ||
| std::thread{std::bind(&GripperReferenceFilterNode::execute, this, | ||
| std::placeholders::_1), goal_handle}.detach(); | ||
| } |
There was a problem hiding this comment.
This node currently uses both a Reentrant callback group for the action server and detaches a thread for the execute function, which IMO is overkill. We also had some race condition issues with this setup in the previous implementation of the reference filter, which is now fixed so you can take inspiration from that.
Comment on lines
+135
to
+140
| x(0) = reference_(0); // roll | ||
| x(1) = reference_(1); // pinch | ||
| x(2) = 0.0; // roll_dot | ||
| x(3) = 0.0; // pinch_dot | ||
| x(4) = 0.0; // roll_dotdot | ||
| x(5) = 0.0; // pinch_dotdot |
There was a problem hiding this comment.
Does the gripper reference message need to be 6d if we ever only use the first two dims?
Comment on lines
+132
to
+168
| Eigen::Vector6d GripperReferenceFilterNode::fill_reference_state() { | ||
| Eigen::Vector6d x = Eigen::Vector6d::Zero(); | ||
|
|
||
| x(0) = reference_(0); // roll | ||
| x(1) = reference_(1); // pinch | ||
| x(2) = 0.0; // roll_dot | ||
| x(3) = 0.0; // pinch_dot | ||
| x(4) = 0.0; // roll_dotdot | ||
| x(5) = 0.0; // pinch_dotdot | ||
|
|
||
| return x; | ||
| } | ||
|
|
||
| Eigen::Vector2d GripperReferenceFilterNode::fill_reference_goal( | ||
| const vortex_msgs::msg::GripperWaypoint& goal) { | ||
|
|
||
| double roll{goal.roll.roll}; | ||
| double pinch{goal.pinch.pinch}; | ||
|
|
||
| Eigen::Vector2d reference; | ||
| reference << roll, pinch; | ||
|
|
||
| return reference; | ||
| } | ||
|
|
||
| vortex_msgs::msg::GripperReferenceFilter GripperReferenceFilterNode::fill_reference_msg() { | ||
| vortex_msgs::msg::GripperReferenceFilter feedback_msg; | ||
|
|
||
| feedback_msg.roll = x_(0); | ||
| feedback_msg.pinch = x_(1); | ||
| feedback_msg.roll_dot = x_(2); | ||
| feedback_msg.pinch_dot = x_(3); | ||
| feedback_msg.roll_dotdot = x_(4); | ||
| feedback_msg.pinch_dotdot = x_(5); | ||
|
|
||
| return feedback_msg; | ||
| } |
There was a problem hiding this comment.
Maybe move these conversion functions to a separate util file to avoid clutter in the ros node
Comment on lines
+237
to
+238
| auto feedback = std::make_shared< | ||
| vortex_msgs::action::GripperReferenceFilterWaypoint::Feedback>(); |
There was a problem hiding this comment.
Should remove feedback from the action definition and just publish to topic if anyone is interested
Comment on lines
+268
to
+270
| vortex_msgs::msg::GripperReferenceFilter feedback_msg = fill_reference_msg(); | ||
| feedback->reference = feedback_msg; | ||
| reference_pub_->publish(feedback_msg); |
There was a problem hiding this comment.
Should use unique pointer for this kind of pipeline style publishing. Ref: https://vortex.a2hosted.com/index.php/Composable_nodes
|
|
||
| GripperReferenceFilterNode::GripperReferenceFilterNode(const rclcpp::NodeOptions& options) | ||
| : Node("gripper_reference_filter_node", options) { | ||
| time_step_ = std::chrono::milliseconds(10); |
There was a problem hiding this comment.
should rename the variable to time_step_ms_ to make it more explicit that it is milliseconds and not seconds. Also would be nice to expose this as a ros param
…awa removing steady state error
Addresses the following PR review comments on
gripper_guidance/src/gripper_reference_filter_ros.cpp:
L53 (jorgenfj): "Consider using lambdas instead of std::bind to avoid
using std::placeholders"
All std::bind+std::placeholders sites converted to lambdas:
subscription callback, action server callbacks (handle_goal,
handle_cancel, handle_accepted), and the detached execute thread
spawn site (now spawns via std::thread{[this, goal_handle]{...}}).
L79 (jorgenfj): "What is this line spacing? I need a vertical monitor
to read this..."
rclcpp_action::create_server(...) call now matches the pattern from
vortex-auv/guidance/reference_filter_dp_quat: terse lambdas
[this](const auto& uuid, auto goal) {...} instead of std::bind with
placeholders separated by blank lines. The whole call now fits in a
single readable block.
L107 (jorgenfj): "For unused function args a more explicit approach
is to comment out the args themselves instead of using (void)."
handle_goal: (void)uuid; (void)goal; removed; signature now uses
/*uuid*/ and /*goal*/.
handle_cancel: (void)goal_handle; removed; signature uses
/*goal_handle*/.
L135 (jorgenfj): "This node currently uses both a Reentrant callback
group for the action server and detaches a thread for the execute
function, which IMO is overkill. We also had some race condition
issues with this setup in the previous implementation of the
reference filter, which is now fixed so you can take inspiration
from that."
Adopted the pattern from
vortex-auv/guidance/reference_filter_dp_quat/src/ros/reference_filter_ros.cpp:
* Removed rclcpp::CallbackGroup cb_group_ and the
CallbackGroupType::Reentrant group.
* Removed UUID-based preemption (preempted_goal_id_, goal_handle_
member, UUID comparisons inside the loop).
* Added std::atomic<bool> preempted_, std::mutex execute_mutex_,
and std::thread execute_thread_ as members.
* Destructor sets preempted_ and joins the thread.
* handle_accepted now: locks execute_mutex_, signals preempt,
joins the prior thread, clears preempt, then spawns the new
execute thread. This eliminates the race the previous
implementation had.
* The execute loop now polls preempted_.load() instead of
comparing goal UUIDs.
Hold-timer behaviour (hold_active_, hold_reference_msg_, hold_timer_)
is preserved unchanged.
L173 (jorgenfj): "Maybe move these conversion functions to a
separate util file to avoid clutter in the ros node"
fill_reference_state, fill_reference_goal, fill_reference_msg, and
apply_mode_logic moved from member functions on the node to free
functions in a new header
gripper_reference_filter/gripper_reference_filter_ros_utils.hpp.
Mirrors the layout in
vortex-auv/.../reference_filter_dp_quat/ros/reference_filter_ros_utils.hpp.
Corresponding member declarations removed from the node header.
L294 (jorgenfj): "Should use unique pointer for this kind of
pipeline style publishing. Ref: ...Composable_nodes"
Every reference_pub_->publish(...) site converted to std::make_unique
+ publish(std::move(...)):
* publish_hold_reference
* publish_hold_timer
* the periodic feedback publish inside execute
* the final-convergence publish inside execute
Enables the zero-copy intra-process path documented in the Composable
Nodes wiki entry.
L258 (jorgenfj) companion to the action-definition trim in
vortex-msgs:
goal_handle->publish_feedback(feedback) removed from execute; the
local feedback shared_ptr removed. The topic publish remains the
single source of reference data.
L24 (jorgenfj): "should rename the variable to time_step_ms_ to make
it more explicit that it is milliseconds and not seconds. Also would
be nice to expose this as a ros param"
NOT addressed in this commit. time_step_ is typed
std::chrono::milliseconds (unit is in the type, not the name) and is
set inside the constructor. Exposing it as a ROS parameter would be
a separate behavioural change; flagged here for follow-up rather
than silently included with the refactor.
Additional changes required by the .msg/.action trim landed in
vortex-msgs (GripperReferenceFilter, GripperWaypoint, action feedback):
* fill_reference_msg now writes only roll and pinch; the dot/dotdot
assignments are gone.
* publish_hold_reference no longer zeroes derivative fields.
* fill_reference_goal reads waypoint_goal.roll / .pinch directly
(no longer .roll.roll / .pinch.pinch).
* The 6D filter_state_ still drives the dynamics internally; only
the wire format shrank.
Member renames for readability (Code Complete ch. 11):
* x_ to filter_state_ (the rclcpp publish-side rename in this file
intentionally does not touch GripperReferenceFilter::calculate_x_dot,
which keeps its mathematical name).
Addresses the PR review comment on
gripper_controller/src/gripper_controller_ros.cpp L127 (jorgenfj):
"These local state variables should preferably be a const. Should look
into this for complex const initialization using lambdas".
Although the comment was filed against the controller, the same const-
correctness principle was applied across both packages. In this file:
* calculate_x_dot: x_dot is now const Eigen::Vector6d. It is computed
once from Ad_*x + Bd_*r and never mutated; const prevents future
accidental writes that would invalidate the return value.
* calculate_Ad: omega_diag, zeta_diag, omega_diag_squared,
omega_diag_cubed are now const Eigen::Matrix2d. All four are
derived from omega/zeta and only ever read when assembling Ad_
blocks; const documents that they are mathematical intermediates.
* calculate_Bd: omega_diag, omega_diag_squared, omega_diag_cubed
likewise const; same reasoning.
No member variables were made const. Ad_ and Bd_ are assembled
incrementally inside calculate_Ad/calculate_Bd and so genuinely require
mutation.
Addresses three PR review comments. Two were filed against
gripper_guidance/src/gripper_reference_filter_ros.cpp but apply equally
here; one was filed directly against this file.
L127 (jorgenfj): "These local state variables should preferably be a
const. Should look into this for complex const initialization using
lambdas"
set_controller_params: proportional_gain_matrix is now const,
initialised via an IIFE lambda that builds the 2x2 zero matrix, sets
the diagonal entries, and returns it. Prevents any later accidental
write to the gain matrix between construction and the
controller_.set_kp(...) call.
set_subscribers_and_publisher: qos_sensor_data is now const.
publish_control: measured_state and reference_state are now const,
obtained via an IIFE lambda that takes state_mutex_, snapshots the
four members into local structs, and returns them as a std::pair.
The lambda's scope owns the lock; the const locals are visible
outside the critical section, which both minimises lock duration and
prevents accidental mutation afterward. velocity_command is also
const.
L53 (jorgenfj): "Consider using lambdas instead of std::bind to
avoid using std::placeholders"
Both create_subscription callbacks (reference_sub_, state_sub_) and
the create_wall_timer callback now use lambdas:
[this](const T::SharedPtr message) { method(message); }
[this]() { publish_control(); }
Lambdas were chosen because: (1) readability, argument intent is
visible at the call site, (2) no std::placeholders::_N ordering bug
surface, (3) explicit capture documents which state the callback
closes over.
L294 (jorgenfj): "Should use unique pointer for this kind of
pipeline style publishing. Ref: ...Composable_nodes"
control_pub_->publish(velocity_command_msg) (lvalue copy path)
converted to std::make_unique<GripperStateVelocityCommand>(...) +
publish(std::move(velocity_command_msg)). header.stamp is set
through the unique_ptr before the move. Enables the zero-copy
intra-process path.
Addresses the PR review comment on
gripper_controller/src/gripper_controller_ros.cpp L127 (jorgenfj):
"These local state variables should preferably be a const. Should look
into this for complex const initialization using lambdas".
In calculate_velocity:
* position_error is now const types::Vector2d, initialised via an
IIFE lambda that constructs the local Eigen vector, writes the two
components, and returns it. Prevents later writes that could
desynchronise the error from the state inputs.
* velocity_command is left non-const intentionally: std::clamp is
used to saturate both components in place after the Kp_ * error
multiplication, so the variable genuinely requires mutation.
Marked here so the asymmetry is not mistaken for an oversight.
jorgenfj
requested changes
May 18, 2026
|
|
||
| GripperControllerNode::GripperControllerNode(const rclcpp::NodeOptions & options) | ||
| : Node("gripper_controller_node", options) { | ||
| time_step_ = std::chrono::milliseconds(10); |
There was a problem hiding this comment.
this is also assigned in set_controller_params()
Also maybe call the variable time_step_ms_ to be more explicit
|
|
||
| private: | ||
| types::Matrix2d Kp_; | ||
| double timestep_; |
|
|
||
| GripperReferenceFilterNode::GripperReferenceFilterNode(const rclcpp::NodeOptions& options) | ||
| : Node("gripper_reference_filter_node", options) { | ||
| time_step_ = std::chrono::milliseconds(10); |
|
|
||
| std::chrono::milliseconds time_step_{}; | ||
|
|
||
| Eigen::Vector6d filter_state_; |
There was a problem hiding this comment.
Maybe the reference filter class should own the filter_state_?
Author
There was a problem hiding this comment.
will look into it tmrw (this for all fresh comments)
Comment on lines
+147
to
+158
| void GripperReferenceFilterNode::publish_hold_timer() { | ||
| auto hold_message = std::make_unique<vortex_msgs::msg::GripperReferenceFilter>(); | ||
| { | ||
| std::lock_guard<std::mutex> lock(mutex_); | ||
| if (!hold_active_ || !reference_pub_) { | ||
| return; | ||
| } | ||
| *hold_message = hold_reference_msg_; | ||
| } | ||
|
|
||
| reference_pub_->publish(std::move(hold_message)); | ||
| } |
Author
There was a problem hiding this comment.
Will update it to make the intention more obvious.
Fixes the issue where after seving a goal of roll to say 3.14, if we send a goal of roll to 0 the action server reports success IMMEDIATELY likely due to assuming a default value of 0 if we DIDN'T hold previous value like we used to
Comment on lines
+195
to
+272
| while (rclcpp::ok()) { | ||
| if (preempted_.load()) { | ||
| publish_hold_reference(); | ||
| result->success = false; | ||
| goal_handle->abort(result); | ||
| spdlog::info("Goal preempted by newer goal"); | ||
| return; | ||
| } | ||
|
|
||
| if (goal_handle->is_canceling()) { | ||
| publish_hold_reference(); | ||
| result->success = false; | ||
| goal_handle->canceled(result); | ||
| spdlog::info("Goal canceled"); | ||
| return; | ||
| } | ||
|
|
||
| const Eigen::Vector6d state_derivative = | ||
| gripper_reference_filter_->calculate_x_dot(filter_state_, goal_reference); | ||
| filter_state_ += state_derivative * time_step_.count() / 1000.0; | ||
|
|
||
| auto reference_message = | ||
| std::make_unique<vortex_msgs::msg::GripperReferenceFilter>( | ||
| fill_reference_msg(filter_state_)); | ||
| { | ||
| std::lock_guard<std::mutex> lock(mutex_); | ||
| hold_reference_msg_ = *reference_message; | ||
| } | ||
| reference_pub_->publish(std::move(reference_message)); | ||
|
|
||
| Eigen::Vector2d current_state; | ||
| { | ||
| std::lock_guard<std::mutex> lock(mutex_); | ||
| current_state = reference_; | ||
| } | ||
|
|
||
| Eigen::Vector2d error = current_state - goal_reference; | ||
| switch (mode) { | ||
| case vortex_msgs::msg::GripperWaypoint::ONLY_ROLL: | ||
| error(1) = 0.0; | ||
| break; | ||
| case vortex_msgs::msg::GripperWaypoint::ONLY_PINCH: | ||
| error(0) = 0.0; | ||
| break; | ||
| case vortex_msgs::msg::GripperWaypoint::ROLL_AND_PINCH: | ||
| default: | ||
| break; | ||
| } | ||
|
|
||
| if (error.norm() < convergence_threshold) { | ||
| filter_state_.head(2) = goal_reference; | ||
| auto final_message = | ||
| std::make_unique<vortex_msgs::msg::GripperReferenceFilter>( | ||
| fill_reference_msg(filter_state_)); | ||
| { | ||
| std::lock_guard<std::mutex> lock(mutex_); | ||
| hold_reference_msg_ = *final_message; | ||
| hold_active_ = true; | ||
| } | ||
| reference_pub_->publish(std::move(final_message)); | ||
| result->success = true; | ||
| goal_handle->succeed(result); | ||
| spdlog::info("Goal reached"); | ||
| return; | ||
| } | ||
|
|
||
| loop_rate.sleep(); | ||
| } | ||
|
|
||
| if (!rclcpp::ok() && goal_handle->is_active()) { | ||
| result->success = false; | ||
| try { | ||
| goal_handle->abort(result); | ||
| } catch (...) { | ||
| // Ignore exceptions during shutdown | ||
| } | ||
| } | ||
| } |
There was a problem hiding this comment.
This function currently does too much. Could move the filter state step and the mode dependent error calculations into separate helpers
|
Would be nice to have a "simple" open loop gripper controller that owns an action server which accepts desired change or rotation, then the controller just publishes this velocity directly for a given amount of time before the action completes. This way we are still able to operate the gripper in the case that feedback is not available |
ApatShe
added a commit
to vortexntnu/vortex-msgs
that referenced
this pull request
May 19, 2026
Required by the open-loop gripper controller added in vortexntnu/vortex-gripper#10. The action accepts a velocity command and a fixed duration. The controller publishes that velocity on GripperStateVelocityCommand at a fixed rate for the specified duration, then stops and completes the goal. Intended as a feedback-free fallback when gripper state is not observable. Goal: float64 roll_velocity float64 pinch_velocity float64 duration_seconds uint8 mode (ROLL_AND_PINCH=0, ONLY_ROLL=1, ONLY_PINCH=2) Result: bool success Feedback: float64 elapsed_seconds
Addresses PR review comments on gripper_controller/src/gripper_controller_ros.cpp (jorgenfj, #10): L24: "this is also assigned in set_controller_params(). Also maybe call the variable time_step_ms_ to be more explicit" Removed the redundant default assignment of time_step_ms_ in the GripperControllerNode constructor. The ROS parameter declaration in set_controller_params() is the single source of truth. Renamed time_step_ to time_step_ms_ in GripperControllerNode to make the millisecond unit explicit in the member name. L28 on gripper_controller/include/gripper_controller/gripper_controller.hpp: "timestep_s_?" Renamed timestep_ to time_step_ms_ in GripperController. The member is not used in the P-controller math (velocity = Kp * error has no time dependency), so the unit rename has no effect on computed output. Setter renamed from set_time_step to set_time_step_ms accordingly. Caller now passes the millisecond integer directly rather than dividing by 1000. Companion: gripper_guidance/config/gripper_reference_filter_params.yaml gains time_step_ms: 10, matching the controller config convention. The reference filter ROS node declares this parameter in set_refererence_filter(); the hardcoded constructor default is removed.
…ilter Addresses PR review comment on gripper_guidance/include/gripper_reference_filter/gripper_reference_filter_ros.hpp (jorgenfj, #10): L73: "Maybe the reference filter class should own the filter_state_?" The 6D filter state [roll, pinch, roll_dot, pinch_dot, roll_dotdot, pinch_dotdot] is now a private member of GripperReferenceFilter rather than the ROS node. The Fossen third-order filter (Handbook of Marine Craft Hydrodynamics and Motion Control, Fossen 2021 p. 336 eq. 12.5-6) requires the full 6D state internally to produce smooth output; only the first two components (roll, pinch) are ever published on the wire. New public API: reset(initial_reference) -- seeds position components, zeros higher order step(goal_reference, time_step_seconds) -- integrates one time step snap_to(goal_reference) -- pins position components at convergence reference_output() -- returns 2D [roll, pinch] for publication calculate_x_dot moved to private; calculate_Ad/calculate_Bd remain public for testability. Tests updated to use the new stateful API.
Addresses PR review comments on gripper_guidance/src/gripper_reference_filter_ros.cpp (jorgenfj, #10): L158: "What is this used for?" Renamed publish_hold_timer to republish_held_reference_tick, hold_timer_ to held_reference_republish_timer_, hold_active_ to holding_reference_, hold_reference_msg_ to last_published_reference_, and publish_hold_reference to latch_current_state_as_held_reference. The doxygen on republish_held_reference_tick explains the invariant it maintains: downstream consumers keep receiving the last committed reference when no goal is active, preventing the convergence check from spuriously succeeding when a new goal targets a value near zero and the measured state defaults to zero. Bug fix (ApatShe, #10): "after serving a goal of roll to 3.14, if we send a goal of roll to 0 the action server reports success IMMEDIATELY likely due to assuming a default value of 0 if we DIDN'T hold previous value" execute() now seeds the filter from last_published_reference_ when holding_reference_ is true, falling back to measured_reference_ only on first use. Convergence is checked against the filter's own 2D output (gripper_reference_filter_->reference_output()), not the measured state, so a zero-valued hardware echo cannot trigger a false success. L272: "This function currently does too much. Could move the filter state step and the mode dependent error calculations into separate helpers" Mode-dependent error masking extracted to compute_convergence_error in gripper_reference_filter_ros_utils.hpp. The filter step is now a single call to gripper_reference_filter_->step(...) following the ownership refactor in the previous commit. L173: fill_reference_goal, fill_reference_msg, and apply_mode_logic updated to work with the 2D reference_output() API. Companion to vortex-msgs branch 584-task-gripper-controller which trims GripperReferenceFilter to roll/pinch only and GripperWaypoint to flat float64 fields (#10).
Addresses standalone PR review comment (jorgenfj, #10): "Would be nice to have a simple open loop gripper controller that owns an action server which accepts desired change or rotation, then the controller just publishes this velocity directly for a given amount of time before the action completes. This way we are still able to operate the gripper in the case that feedback is not available" GripperOpenLoopControllerNode accepts a GripperOpenLoopCommand goal (roll_velocity, pinch_velocity, duration_seconds, mode) and publishes GripperStateVelocityCommand at time_step_ms intervals until the duration elapses, then publishes a zero-velocity stop command and succeeds. Supports the same ROLL_AND_PINCH/ONLY_ROLL/ONLY_PINCH mode mask as the closed-loop path. Action type GripperOpenLoopCommand is defined in the companion vortex-msgs branch 584-task-gripper-controller. Package structure mirrors gripper_controller: component library + standalone executable, composable node registration, launch file, and YAML config. Preemption uses the same atomic+thread pattern as reference_filter_dp_quat and the updated gripper_reference_filter. Example goal (pinch only, 3 seconds): ros2 action send_goal /nautilus/gripper/open_loop_controller \ vortex_msgs/action/GripperOpenLoopCommand \ "{roll_velocity: 0.0, pinch_velocity: -0.5, duration_seconds: 3.0, mode: 2}"
gripper_controller.cpp: renamed error -> position_difference inside the IIFE that builds position_error. Single-letter and abbreviation-style names do not communicate intent (CC ch. 11: names should describe what the variable represents). gripper_controller_ros.cpp: renamed matrix -> gain_matrix inside the proportional gain IIFE; renamed measured/reference -> measured_gripper_state/reference_gripper_state inside the publish_control IIFE; renamed lambda params msg -> reference_filter_msg and gripper_state_msg to match the callback method signatures. gripper_controller_ros.hpp: updated callback declarations to match. gripper_reference_filter_ros_utils.hpp: renamed error -> convergence_error inside compute_convergence_error so the local variable name matches the function's documented purpose and does not shadow the semantic meaning of the return value.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
3 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
THE CODE SHOULD SPEAK FOR ITSELF inshallah