Enforcing position limits in the VelocityJointSaturationHandle.

[airballking]

The VelocityJointSaturationHandle does not enforce position limits. I think it should. I implemented a fix that is similar to the way that the VelocityJointSoftLimitsHandle enforces position limits.

Note: I implemented this to have proper joint limit enforcing in ros_control_boilerplate when using velocity-resolved simulation mode. My fix for PickNikRobotics/ros_control_boilerplate#8 needs this pull request.

[bmagyar]

I'm not too deep into the limit interfaces but this looks reasonable at first glance.

Anyone up for a second glance? @efernandez @davetcoleman @ipa-mdl @ipa-fxm ?

[mathias-luedtke]

Why don't you use VelocityJointSoftLimitsHandle?
The VelocityJointSaturationHandle is not meant for this.

At first glance your implementation might reverse the velocity at the boundaries.
I don't think that this is correct for all use cases.

[airballking]

@bmagyar @ipa-mdl thanks for the quick and constructive comments!

I did not choose to use VelocityJointSoftLimitsHandle because that class enforces limits using the values from taken from the safety-controller-tag, and it multiplies with the p-gain from that tag. That did not fit my use-case.

Why is the VelocityJointSaturationHandle not meant to also enforce position limits? I think VelocityJointSaturationHandle is the only joint limit interface that does not enforce position limits. Hence, I thought it was an oversight, and went ahead providing the pull request.

Regarding "your implementation might reverse the velocity at the boundaries", I am not sure what exactly you mean. Could you please provide an example? Then, I can try to improve the pull request.

Again, thank you for your feedback! :)

[mathias-luedtke]

I did not choose to use VelocityJointSoftLimitsHandle because that class enforces limits using the values from taken from the safety-controller-tag, and it multiplies with the p-gain from that tag. That did not fit my use-case.

VelocityJointSoftLimitsHandle reduces the velocity the closer an axis approaches the limit in a configurable manner.
This prevents overshoots associated to full stops.

Your code replicates the behaviour with k_position=1 fixed.

Regarding "your implementation might reverse the velocity at the boundaries", I am not sure what exactly you mean. Could you please provide an example? Then, I can try to improve the pull request.

Your code does not cover the overshoot case properly.
(I don't think there is generic counter-measure for this)
I am not sure if I am right with claiming that your code reverse the motion, this needs a detailed review.

VelocityJointSoftLimitsHandle does not need to handle this case because the softlimits are defined in a way that prevents the overshoot.

[davetcoleman]

I think I side with @airballking that, even if the resulting behavior is not ideal, having position limits for the velocity joint saturation handle is important. I'd rather have a hard stop than no stop at all. I also think its inconsistent that the effort saturation handle has a hard stop position limit but velocity does not.

@ipa-mdl it sounds like your bigger issue is that the non-soft saturation handles exist all for all of the control types. I agree that non-soft is far less ideal and should be avoided, but if they exist they should do as promised and prevent your joint from going beyond its limits. This functionality is especially helpful for simulation, the origin of this issue.

[davetcoleman]

I like your concept but I think the implementation is wrong for the general case

[davetcoleman]

these two lines seem like they have incorrect behavior... I think you copied these from the PositionJointSoftLimitsHandle but flipped the subtraction of the position and position limit, resulting in incorrect units being used / arbitrary values.

I would assume you want a hard-stop position limit like how the effort is implemented, here

[bmagyar]

Hi,
Please add a test to nail this issue. This is the best way to put your point into perspective. (Also we won't merge without it :))