JointLimits violated in simulation mode velocity

[airballking]

Hi,

this is a great package. Thanks for sharing it! I made a package providing similar functionality similar, but this is cooler! So, I try to switch now.

When playing with the velocity-resolved simulation mode, I discovered that joint limits were not obeyed.

My use case is: I have a prismatic joint with limits (a torso) and send commands of 5cm/s to a controller that just forwards the command. When looking at it in RVIZ, the joint moves nicely upwards with the correct velocity. Unfortunately, it never stops, and the joint just "takes off".

Best,
Georg.

[airballking]

I think I found a working fix. Basically, the method sim_hw_interface::enforceLimits(ros::Duration& period) always calls the PositionJointSaturationInterface to enforce joint limits. No matter what the selected simulation mode is.

Adding another switch-case-statement there is no problem because the various interfaces are already registered in the generic_hw_interface.

However, there is a catch. I think VelocityJointSaturationInterface is incomplete: It only enforce velocity and effort limits. I do not know why. The VelocityJointSoftLimitsInterface has an extended version of the same algorithm to also enforce joint position limits. I have implemented a fix for that as well, and will shoot a pull request to that repository, too.

Finally, I also realized that generic_hw_interface does not check whether the default initial position of 0.0 violate joint limits. My robot has such joints. I added a check for that too.

I'll open a pull request, soon.

[davetcoleman]

Thanks for reporting this @airballking !