UFE Wrist Support

Description

Miscellaneous changes needed for UFE features to work with Burt Wrist (Teneo).

I made a lot of stylistic changes to the Dynamics.cs file, mainly to match the organization I've gotten used to of all public properties at the top, then public functions, then private variables, then private functions, etc. I also changed a few variable names, made some things into auto properties (no explicit private variable needed for the property), and got rid of some use of the keyword "ref" when passing forward kinematics, since it is not needed to be passed as a reference. The important change is that I added DynamicsDegreesOfFreedom to track the DOF we are using in dynamics calculations and the size of the inputs and outputs.

There are also a decent number of changes in the DisturbanceObserver.cs and ForwardKinematics.cs files that amount to some variable name changes and the additional handling of the DynamicsDegreesOfFreedom.

Changes to the way we are handling DOF in our fwd kin and dynamics calculations trickle up to the robot controller, so many changes there to ensure vector sizes match. I also wanted to keep the ability to set dynamics parameters from files, so I've made sure that still works through the DisturbanceObserverTest.

I've broken down whether or not the UFE is "valid" into "arm" and "wrist" components, since they shouldn't effect each other.

Also included bugfix for BurtBackgroundMonitor, which just required overloading the SystemLogger logging functions to not try to pass a single string without arguments through string.Format().

What to focus on

• Using DisturbanceObserverTest, is the user wrist torque estimate working (use keypress to toggle wrist torque from robot) and does J4 joint stop correctly disable the user wrist torque