Feature/work est

Description

I did a quick overhaul of the MovementAnalysis class, whose only purpose until now was to calculate endpoint/tool odometry. (I use "endpoint" and "tool" interchangeably throughout the code base, sorry.) Now, it also calculates the work when the disturbance observer is turned on.

I made several classes in RunningStatistic.cs to do much more general calculations on a continually updating signal, such as our robot endpoint position. The idea is we can build off of this to calculate other metrics in the future.

The base class called RunningStatistic is really meant to just encapsulate the correct behavior for values that you want to be able to pause, unpause, and reset. For example, anything that you only want to measure while a game is being played.

The kind of middle-man classes of RunningStatisticScalar and RunningStatisticVector are basically just applying filtering if we need it, so in the future if I were to redesign this I would probably do that differently. Similarly, the way the vector versions can accept one or two inputs is not the cleanest or most elegant solution, but I think it's all good enough for now. Also, I tend to make a "scalar" and "vector" version of most things like this, even if I'm only using one of them, so I don't have to make it in the future. That's why there's RunningStatisticScalar and RunningStatisticVector, etc.

The final things I calculate are ToolWork in Newton-meters and UserEffort in Joules, which are interchangeable units, but the latter implies energy. @tn and I discussed this, and he suggested that Calories might be the most useful units for reporting effort.

What to focus on

• Verify the math used in calculating the line integral for tool odometry and work.
• Use Sandbox.EndpointOdometryTest to verify that odometry still works correctly. (Try moving slow, fast, all around the workspace. Try pausing, unpausing, and resetting in all different combinations a lot.)
• Use Sandbox.DisturbanceObserverTest to verify that the "Tool Work" and "User Effort" values make sense. IMPORTANT: Use the "Haptic Disturbances" option to create a haptic wall that you can press against. When you are moving into the wall, the work value should go up. When you are applying force but still, it should stay the same. When you are moving out of the wall, the work value should go down. When you do a full cycle of pressing in and coming back out of the wall, the work value should return to pretty close to where it was. Conservation of energy, yada yada yada.... The user effort, on the other hand, will only go up.