Description
The disturbance observer uses the inverse of the Jacobian transpose to calculate the estimated disturbance force at the endpoint from the estimated disturbance in the joint torques. There are a few points in the Burt's workspace where this inversion is numerically unstable. Fortunately, this only occurs when the endpoint lies near the robot's Y-axis- which is located inside of where the user should be sitting.
I also replaced the inefficient calculation of x = A.Inverse() * b
with the much faster MathNet.Numerics function x = A.Solve(b)
.
What to focus on
- Use the DisturbanceObserverTest in the Sandbox to see when the disturbance estimate is valid. Press "g" to enter debugging mode where we calculate the condition number of the Jacobian transpose.
- Does the placement of the X position cutoff
kMinXPosValid
make sense?