Speed Scaling - Is there a plan to extend to velocity/acceleration-based interfaces?

[nicolapedrocchi]

Is your feature request related to a problem? Please describe.
The current implementation of speed scaling only works for position-based interfaces.

Describe the solution you'd like
For example, the speed scaling cannot be used when the robot accepts position, feedforward velocity, and acceleration as input. It would be great to allow the possibility of using it in such a context.

Describe alternatives you've considered
In my understanding, modifying the sample function of the trajectory should be enough to support the speed scaling to a velocity and acceleration-based interface.

Additional context
I would be willing to support such implementation.

[christophfroehlich]

We discussed this briefly but didn't want to block to merge of the now existing scaling.

The remaining question was: How should the sampling work for velocity (and acceleration) values? Speed scaling=0.5 means half of the velocity, but acceleration? If you have a solution for that, let's discuss it.

[nicolapedrocchi]

I may be wrong, but the square of the factor should scale it.

Specifically, suppose we have a law of motion over a time interval $T_1$.
Now, we want to achieve the same motion $\bar{s}$ in $T_2$ such that $T_2 = \lambda T_1$ maintaining the same shape, i.e., $s_2(t) = s_1 (t / \lambda)$ for each $t$.

When differentiated $n$ times with respect to time, we get $\frac{d^n}{dt^n} s_2(t) = \frac{1}{\lambda^n} \frac{d^n}{dt^n} s_1 \left(\tfrac{t}{\lambda}\right)$, that turns in $\frac{d}{dt} s_2(t) \equiv v_2(t) = \frac{1}{\lambda} v_1\left(\tfrac{t}{\lambda}\right)$ and $\frac{d^2}{dt^2} s_2(t) \equiv a_2(t) =\frac{1}{\lambda^2} a_1\left(\tfrac{t}{\lambda}\right)$

In my understanding, we should modify only the function 'interpolate_between_points()', adding the scaling factor as input.

Finally, I am sorry I've opened this enhancement discussion under the wrong package ... I should have opened it under ros2_controllers! Have I to close this discussion and reopen it there?

[christophfroehlich]

If the scaling factor remains constant theoretically yes, that was my initial guess as well. but @fmauch had some concerns #301 (comment)

[nicolapedrocchi]

Hi, sorry for the late reply
OK, I've always worked in quasi-static applications, anyway, I agree with you. A simple (brutal) scaling algorithm could be the following:

Let:

• s₂(t) = s₁(τ) (1)
• v₂(t) = v₁(τ) τ̇ (2)
• a₂(t) = a₁(τ) τ̇² + v₁(τ) τ̈ (3)

Where:

• τ̇ is the target scaling factor received as input for the system.

The system must satisfy the following constraints:

• 0 ≤ τ̇ ≤ 1.0 (5)
• −v_max ≤ v₁(τ) τ̇ ≤ v_max (6)
• −a_max ≤ a₁(τ) τ̇² + v₁(τ) τ̈ ≤ a_max (7)

Therefore

i ← i + 1 // The superscript tracks the control loop iteration
τ̇ᵢ ← SATURATION(EXP_FILTER(target_scaling))
OK ← False
while not OK do
τᵢ ← τᵢ₋₁ + τ̇ᵢ (tᵢ − tᵢ₋₁)
k, k₋₁ ← get_trajectory_interval(τᵢ)
if v_k(τᵢ) τ̇ᵢ ≤ v_max then
   τ̈ᵢ ← (τ̇ᵢ − τ̇ᵢ₋₁) / (tᵢ − tᵢ₋₁)
   if a_k(τ) τ̇² + v_k(τ) τ̈ ≤ a_max then
    OK ← True
   else
    τ̇ᵢ ← 0.9 τ̇ᵢ // THIS IS BRUTAL, but in about 20 iterations it should converge in any condition
   end if
  else
   τ̇ᵢ ← v_max / v_k(τᵢ)
  end if
end while

If it is OK with you, I can implement it and test it (I have already done some preliminary evaluations in my fork) - the PR I guess, will consist of:

• functions to read the limits (taking as examples other controllers)
• manage the filter param
• adding a header/src with the main code of the algorithm
• implements all the tests (maybe adding a test in the unit)

However, I have some doubts about the code regarding the state interface you used for the scaling and its relation to the publisher.
Should this controller be chainable? In such a case, the PR will be quite invasive since I guess that I have to split the controller update function into two functions. Am I right?

[christophfroehlich]

I have to check the math above, give me some time.

However, I have some doubts about the code regarding the state interface you used for the scaling and its relation to the publisher. Should this controller be chainable? In such a case, the PR will be quite invasive since I guess that I have to split the controller update function into two functions. Am I right?

There are no plans do make JTC chainable, because right now it does not make lot of sense expecting a trajectory message as input. I haven't thought about chaining something in front of the speed scaling interface, maybe yes?

@fmauch @urfeex any comments from your side on that?

[nicolapedrocchi]

Yes, I was referring only to chaining the scaling interface.
To the best of my understanding, now you can read the scaling from a topic or from an interface (i.e., std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> scaling_state_interface_;), which seems to me to be the approach of a ChainableController.
I probably still haven't understood the implementation well, but having these two input options in parallel could create some weird behavior.
I guess having a chainable support for the scaling interface could ease the implementation of controllers for SSM (Speed Separation Monitoring).

[christophfroehlich]

the logic for a chainable controller is the update_reference_from_subscriber if it is not in chained mode. I'm not sure now if this is supported to have some subscribers from topic, and some from preceding controllers. Currently, idk why it shouldn't.

[urfeex]

At least from a UR's standpoint velocity-based scaling works with the concept that we currently have. The question is: What do you want to send as a robot's command?

Let me elaborate from a UR's perspective. Speed scaling can happen here because of two reasons:

1. The commands sent to the robot would violate the robot's safety settings. E.g. a large rotation of the shoulder with a bigger robot in an extended configuration will lead to a large Cartesian velocity at the tool. The robot slows down the commanded motion in that case.
2. The speed slider of the teach pendant is set to a percentage less than 100%. In that case, every incoming command is slowed down to match what you described above.

Those two can be combined, meaning that the same command that would cause a 50% slowdown because of the safety system will get slowed down additionally by the speed slider position. So, if the speed slider is at 50%, the motion would actually be executed at 25%.

That's why on URs we report the product of the internal speed reduction and the speed slider as a speed scaling factor through the hardware interface.

Since speed scaling on the robot works that way, it is important to feed the unscaled command to the robot. That's the reason why we don't scale sampling time for the end of the time step, but sampling time for the beginning of the time step. That means, with a scaling factor of 0.5 we assume the robot moved only 50% of the commanded position deviation and we command a 100% step to the robot.

That concept of scaling should work fine with velocities, as well, though the actual path would probably be slightly different due to neglecting the robot's dynamics.

For accelerations the same thing might hold up, but I would expect that the robot's dynamics play an even bigger role.

If you now take a robot that doesn't do that kind of scaling, the position-based approach still works, since the commanded position is always sampled 50% after the previous one (please excuse this fuzzy wording, but I think that explains the concept).

With velocities, however, it's a totally different story. For a robot not doing any scaling by itself, you would actually want to command the fraction of the velocity instead.

That is the main reason why we restricted it to positions, as this was the kind of common domain that should work for both.

If we want to add velocity and acceleration-based scaling, I think we would have to support different scaling policies, at least "scaling on robot" and "scaling in ROS controller".

These are the concerns that I had in my earlier comment, thank you for pinging me on this.