[jog_arm] Changes before porting to ROS2

[AdamPettinger]

Description

These are changes I wanted to make (in addition to #2103) before starting on porting the jogger to ROS2 because I want these changes there as well.

The changes consist of a lot of minor changes, see list below for details. Note that discussion and reviews exist here already: PickNikRobotics#8, because I wanted to start the review process before #2103 landed.

• General cleanup: defining const in functions where applicable, fixing comments, combining if statements and making them flow better
• I found a stale but nonzero command could make it through the timer callback function without updating the joint position filters, so fixed that and made sure the filters are updated every cycle
• Removed the suddenHalt(Eigen::ArrayXd& delta_theta) overload. It included comments saying position and velocity controlled robots were handled differently, but this function did not handle them differently (and only set delta_theta to all 0 which could be really bad for a position controlled robot). It was only called one place with the Eigen Array, and I replaced the call with a setZero() there and deleted the function so future users don't use it and accidentally send the robot to 0's.
• Small fixes in the drift dimensions use and in singularity scaling. Previously the singularity scaling wasn't doing what the algorithm in the comments said it was. Tested after fixing and it looks really good
• Small changes around the have_nonzero_twist_stamped_ and have_nonzero_joint_jog_ which were used intermittently in the run() timer callback but also set in the command subscriber callbacks. Moved them to mutex protected implementation similar to the rest of subscriber callback variables
• Avoid recalculating the planning -> command transform if the incoming command is empty or in the command frame

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[tylerjw]

I've tested this locally.

[codecov]

Codecov Report

Merging #2151 into master will decrease coverage by 0.31%.
The diff coverage is 81.33%.

@@            Coverage Diff             @@
##           master    #2151      +/-   ##
==========================================
- Coverage   58.27%   57.96%   -0.32%     
==========================================
  Files         327      327              
  Lines       24707    25668     +961     
==========================================
+ Hits        14399    14879     +480     
- Misses      10308    10789     +481     

Impacted Files	Coverage Δ
...og_arm/include/moveit_jog_arm/jog_arm_parameters.h	100.00% <ø> (ø)
.../moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h	100.00% <ø> (ø)
...xperimental/moveit_jog_arm/src/collision_check.cpp	80.26% <ø> (ø)
...veit_experimental/moveit_jog_arm/src/jog_calcs.cpp	73.70% <81.33%> (+2.60%)	⬆️
.../move_group_interface/src/move_group_interface.cpp	48.07% <0.00%> (-51.93%)	⬇️
...meterization/work_space/pose_model_state_space.cpp	82.31% <0.00%> (-0.69%)	⬇️
moveit_core/robot_state/src/robot_state.cpp	47.39% <0.00%> (-0.50%)	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	70.10% <0.00%> (-0.15%)	⬇️
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	79.37% <0.00%> (+0.44%)	⬆️
... and 3 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 9e4a164...0e9cda0. Read the comment docs.

[tylerjw]

I'm not sure why but locally testing something is weird happening with this PR. I'll post more when I know more.

[tylerjw]

Something to do with resetting the low pass filters is causing some weird behavior in one of our projects. I'll investigate more tomorrow and try to figure out why.

[AdamPettinger]

Recent update is rebased on master to grab Tyler's #2155, and includes changes:

1. I switched to tracking the "staleness" of joint jog commands and Cartesian twist commands separately. We were getting into trouble when the command was non-stale because of a joint jog command, but the last (and stale) twist command was nonzero, resulting in doing Cartesian jogging calcs on the stale command and ignoring the joint jog (because we prioritize that)
2. One of the problems @tylerjw and @AndyZe were seeing was a bad output trajectory. Tracked it to when no jogging calcs were being performed, and decided to send the previous positions and 0 velocity in this case
3. Slight fix to adding redundant trajectory points if publishing to Gazebo
4. Fixed a stupid Eigen Vector initialization on my part that was causing wild and undesirable joint changes while joint jogging

[AndyZe]

See my PR to correct a variable type in addJointIncrements()

[AndyZe]

I don't see any performance issues. Left a couple small comments but I'm approving assuming you'll fix those