Why is `penality_epsilon` a flat error instead of relative?

[VRichardJP]

penalty_epsilon is used in the edges to as safety margin. The value is flat (eg. 0.15) and used for almost everything. For example, if max speed max_vel_x is set to 10 m/s, no cost will be introduced for speed up to 9.85 m/s. If on the other hand we consider rotation speed max_vel_theta, values are usually way lower (eg. 0.5 rad/s), which means that suddenly the penalty_epsilon value has become way more important (no error up to 0.35 rad/s). It also introduces weird issues when the penalty_epsilon ends up being bigger than robot parameters.

Wouldn't it make more sense to use penality_epsilon as a relative error? For example, if penalty_epsilon is set to 0.1, then it means 10% relative error is acceptable. For example:

• if max_vel_backwards=2.0 and max_vel_x= 10.0, the penaltyBoundToInterval function would not introduce cost for speed values between -1.8 and 9.0 m/s
• if max_vel_theta=0.5, the penaltyBoundToInterval function would not introduce cost for angular speed between -0.45 and 0.45

[VRichardJP]

By extension, the penalty cost could be relative to the bound interval too. To go back to my previous example (max_vel_x=10.0 and max_vel_theta=0.5):

• if one node has a speed of 10.5 m/s, a penalty of 0.5 is introduced
• if one node has an angular speed of 1.0 rad/s, a penalty of 0.5 is introduced

Obviously, in the second case, the excess angular speed is a way bigger problem than the extra 0.5m/s longitudinal velocity. However if both associated weights are the same, they are considered as equivalent from the optimization perspective. Most likely the optimizer will not bother fixing the angular speed.

The current solution is to increase weight_max_vel_theta. For example if I want both velocity and angular speed errors to be "equally important" in my example, I could use weight_max_vel_x=1 and weight_max_vel_theta=20. With such weight, a 10m/s extra longitudinal speed is as bad as a 0.5 rad/s excess angular speed.

This feels really counter-intuitive to me. If I see weight_max_vel_x=1 and weight_max_vel_theta=20 I would definitely think that angular speed has been made way more important than longitudinal speed. To understand they actually weight the same, I would need to look at the vehicle parameters and also check how the penalty value is calculated in the code. I'm pretty sure the average user should not be expected to read the code to understand how to tune the weights.

If instead the penalty is made relative to the bound interval, the previous example becomes:

• if one node has a speed of 10.5 m/s, a penalty of 0.05 is introduced (5% error)
• if one node has an angular speed of 1.0 rad/s, a penalty of 1.0 is introduced (100%error)

And the optimizer would immediately see that the angular speed has too be optimized first.

[Dhivin]

@VRichardJP Relative penality_epsilon is a good idea. I ended up introducing more penality_epsilon configs for the constraints i wanted to further fine tune. So i have like 4 different penality_epsilon values which is a bit messy.