Implement realtime Ruckig jerk-limited smoothing

[AndyZe]

Description

Implement jerk-limited smoothing in Servo. This should enable large industrial robots to be controlled in a realtime fashion.

Checklist

• Extend the tutorials / documentation -- I'd like to make a new tutorial about these smoothing plugins since there are 3 of them now.

Test Instructions

Note this change in panda_simulated_config.yaml: smoothing_filter_plugin_name: "online_signal_smoothing::RuckigFilterPlugin"

That's the only change the user needs to make. Then run one of the Servo demos like normal, e.g.

ros2 launch moveit_servo demo_ros_api.launch.py

ros2 run moveit_servo servo_keyboard_input

[AndyZe]

@ibrahiminfinite

[sea-bass]

Thanks for trying out the optional jerk limits! Hopefully the issue you raised there will go somewhere in future.

There seems to be a failing clang-tidy check. Once that is clear (and if you're ready), request review and I'll give it another look.

[sea-bass]

Requesting changes because I think those CI / clang-format changes should not be part of this PR.

---

Also, I see this PR has components of #2954 -- do you want to merge that in first, or is the plan to combine both of these into this PR?

Either way, my comment on that other PR stands, whether the changes go in here or there first. I'd like to see validation that the fixes in the smoother logic don't break the process of switching between free-space motion planning and servo, without sudden jumps due to old smoother state.

---

Finally, if you do plan to switch the default panda_simulated_config.yaml to use the Ruckig plugin, all the launch files in the demos and integration tests need to update their parameter definitions (or maybe not, if they have sensible defaults)

[sea-bass]

Just one tiny line to fix, LGTM

[AndyZe]

We both weren't really satisfied with the bit of code below so I tried 4 different ways to handle it. There is some info on how Ruckig handles this tracking scenario here: https://github.com/pantor/ruckig?tab=readme-ov-file#tracking-interface

1. My first implementation: Assume zero vel/accel at target state. Seems very smooth but the direction of motion of the robot is not completely correct. (It should be vertical.) In short I think this is due to longer intervals before the target state would be reached. In the given control period (0.01s) the target state wasn't reached so the direction of motion changed.

  // Update Ruckig target state
  // This assumes stationary at the target (zero vel, zero accel)
  ruckig_input_->target_position = std::vector<double>(positions.data(), positions.data() + positions.size());
  // target velocities and accelerations are zero

stationary_at_target.webm

2. The Ruckig way, assume the current acceleration continues through the next target state. Seemed jerky. This method doesn't make sense to me.

ruckig_way_maintain_current_accel.webm

3. This seemed the best. Smooth and the direction of motion was good.

  ruckig_input_->target_position = std::vector<double>(positions.data(), positions.data() + positions.size());
  // We don't know what the next command will be. Assume velocity continues forward based on current state,
  // target_acceleration is zero.
  const size_t num_joints = ruckig_input_->current_acceleration.size();
  for (size_t i = 0; i < num_joints; ++i)
  {
    ruckig_input_->target_velocity.at(i) =
        ruckig_input_->current_velocity.at(i) + ruckig_input_->current_acceleration.at(i) * params_.update_period;
  }
  // target_acceleration remains a vector of zeroes

zero_accel_target.webm

4. This was noticeably jerky and got stuck at singularities (overshot the target position, I assume)

  ruckig_input_->target_position = std::vector<double>(positions.data(), positions.data() + positions.size());
  // We don't know what the next command will be, so assume acceleration tends to zero
  const size_t num_joints = ruckig_input_->current_acceleration.size();
  for (size_t i = 0; i < num_joints; ++i)
  {
    ruckig_input_->target_velocity.at(i) =
        ruckig_input_->current_velocity.at(i) + ruckig_input_->current_acceleration.at(i) * params_.update_period;
    if (ruckig_input_->current_acceleration.at(i) > 0)
    {
      // Positive acceleration, takes a step toward zero
      ruckig_input_->target_acceleration.at(i) =
          ruckig_input_->current_acceleration.at(i) - ruckig_input_->max_jerk.at(i) * params_.update_period;
    }
    else
    {
      // Negative acceleration, takes a step toward zero
      ruckig_input_->target_acceleration.at(i) =
          ruckig_input_->current_acceleration.at(i) + ruckig_input_->max_jerk.at(i) * params_.update_period;
    }
  }

step_toward_zero_accel_target.webm

So i'm going with number 3