584 task gripper controller merge into main we want juicy gripper inside of main by ApatShe · Pull Request #10 · vortexntnu/vortex-gripper

ApatShe · 2026-04-23T13:34:25Z

THE CODE SHOULD SPEAK FOR ITSELF inshallah

…nversion

…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

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.

jorgenfj · 2026-04-26T19:00:39Z

+  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_;
+  }


These local state variables should preferably be a const.
Should look into this for complex const initialization using lambdas

jorgenfj · 2026-04-26T19:03:05Z

+    reference_sub_ = this->create_subscription<vortex_msgs::msg::GripperState>(
+        gripper_state_topic, qos_sensor_data,
+        std::bind(&GripperReferenceFilterNode::reference_callback, this,
+                  std::placeholders::_1));


Consider using lambdas instead of std::bind to avoid using std::placeholders

jorgenfj · 2026-04-26T19:03:53Z

+    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_);


What is this line spacing? I need a vertical monitor to read this...

jorgenfj · 2026-04-26T19:06:19Z

+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;


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*/

jorgenfj · 2026-04-26T19:08:24Z

+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();
+}


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.

jorgenfj · 2026-04-26T19:12:26Z

+    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


Does the gripper reference message need to be 6d if we ever only use the first two dims?

jorgenfj · 2026-04-26T19:13:21Z

+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;
+}


Maybe move these conversion functions to a separate util file to avoid clutter in the ros node

jorgenfj · 2026-04-26T19:14:32Z

+    auto feedback = std::make_shared<
+        vortex_msgs::action::GripperReferenceFilterWaypoint::Feedback>();


Should remove feedback from the action definition and just publish to topic if anyone is interested

jorgenfj · 2026-04-26T19:15:38Z

+        vortex_msgs::msg::GripperReferenceFilter feedback_msg = fill_reference_msg();
+        feedback->reference = feedback_msg;
+        reference_pub_->publish(feedback_msg);


Should use unique pointer for this kind of pipeline style publishing. Ref: https://vortex.a2hosted.com/index.php/Composable_nodes

jorgenfj · 2026-04-26T19:19:18Z

+
+GripperReferenceFilterNode::GripperReferenceFilterNode(const rclcpp::NodeOptions& options)
+    : Node("gripper_reference_filter_node", options) {
+    time_step_ = std::chrono::milliseconds(10);


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

forrisdahl and others added 15 commits September 8, 2025 17:20

refactor: renamed from encoder_read to read_encoders

0c60579

refactor: added num_servos constant and simplified uint16 to uint8 co…

92ae9de

…nversion

initial: initial commit

b906d74

refactor: remove exceptions, send data in little endian

0fa7bd3

fix: wrong offset in memcpy

7921014

moved controller module related files originally made in interface fo…

af50e2c

…lder as well as removed the previously added dependencies as consequence of making the controller module within the interface

Moved controller module to new, dedicated folder, added contents were…

92e2ac6

… originally in the gripper_interface/

feat: add can interface

c304c50

feat: convert from i2c to can

2c3f684

feat: gripper controller skeleton implemented, builds and can launch …

429ee30

…with entire system, needs actual servo states to simulate proper topic relationships

can -adjusted gripper interface n driver

8adf510

Merge remote-tracking branch 'origin/refactor/gripper_driver' into 58…

4cd64cf

…4-task-gripper-controller

feat: implemented skeleton for can hardware interface a.w.a gripper s…

71848df

…im interface with functioning goal sends to ref filter playing out in sim environment

idk

11842fe

ApatShe requested review from CurryMonsters, Q3rkses, forrisdahl and jorgenfj April 23, 2026 13:34

jorgenfj requested changes Apr 26, 2026

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

584 task gripper controller merge into main we want juicy gripper inside of main#10

584 task gripper controller merge into main we want juicy gripper inside of main#10
ApatShe wants to merge 15 commits intomainfrom
584-task-gripper-controller

ApatShe commented Apr 23, 2026

Uh oh!

jorgenfj left a comment

Uh oh!

jorgenfj Apr 26, 2026

Uh oh!

jorgenfj Apr 26, 2026

Uh oh!

jorgenfj Apr 26, 2026

Uh oh!

jorgenfj Apr 26, 2026

Uh oh!

jorgenfj Apr 26, 2026

Uh oh!

jorgenfj Apr 26, 2026

Uh oh!

jorgenfj Apr 26, 2026

Uh oh!

jorgenfj Apr 26, 2026

Uh oh!

jorgenfj Apr 26, 2026

Uh oh!

jorgenfj Apr 26, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

		auto feedback = std::make_shared<
		vortex_msgs::action::GripperReferenceFilterWaypoint::Feedback>();

Conversation

ApatShe commented Apr 23, 2026

Uh oh!

jorgenfj left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants