Integrate Invariant State Estimator into Framework - #1413
Conversation
start writing invariant state
full factory and sim test up next
- finds correctly named IMU - publishes CoM information to the controller
… into new-estimator/joint-level-fixes
…new-estimator/joint-debug
|
I would strongly prefer this be implemented as additional option mode, rather than a replacement. At the moment, you're replacing modules (e.g. the imuBiasStateEstimator). The estimator itself already has running modes baked into it (FROZEN and NORMAL). You could introduce a third (EKF) mode, and a fourth (BOTH) mode, and resolve what is going on between the two that way. Specifically for what I pointed out, the interfaces enable this quite easily (both the imuBiasStateEstimator and the preFilter implement IMUBiasProvider) |
| cancelGravityFromAccelerationMeasurement, | ||
| estimatorDT, | ||
| parentRegistry); | ||
| case JOINT_KF -> JointLevelKFPreFilter.createForKinematicsEstimator(sensorOutputMap, |
There was a problem hiding this comment.
I'd really like to move away from factories. I don't see how this style of code provides any advantage over directly calling the constructor.
| massMatrixNuisanceColumns[i] = nuisanceColumns.get(i); | ||
| nuisanceRotorInertiaDiag[i] = nuisanceRotor.get(i); | ||
| } | ||
| numNuisanceDoF = massMatrixNuisanceColumns.length; // 6 + gap joints |
There was a problem hiding this comment.
don't abbreviate words, that provides no advantage and goes against our programming style.
| "HIP_X", "HIP_Z", "HIP_Y", "KNEE", "ANKLE_Y", "ANKLE_X", "SPINE", | ||
| "SHOULDER_Y", "SHOULDER_X", "SHOULDER_Z", "ELBOW", | ||
| "WRIST_Z", "WRIST_X", "GRIPPER_Z", "NECK_Z", "NECK_Y"}; | ||
| private static final double[] ROTOR_INERTIA_VALUES = { |
There was a problem hiding this comment.
This should absolutely not exist in here.
| private static final double COND_S_MAX = 1.0e9; | ||
| private static final double INIT_POS_VAR = 1.0e-6; // encoders trusted at initialization | ||
| private static final double INIT_VEL_VAR = 1.0; // velocity unknown at initialization | ||
| private static final double INIT_BIAS_VAR = 2.5e-3; // (0.05 rad/s)^2 |
There was a problem hiding this comment.
All this stuff should be extracted into a parameters class, and then used to create yo variables that can be tuned on the fly within this filter.
| // Package-private (not private) so the allocation/behavior tests in this package can build it directly | ||
| // from a synthetic IMU-pair setup without standing up a full StateEstimatorParameters. | ||
| /** Scalar-CWNA process-noise overload: no robot model, so Qa = SIGMA_ACCEL^2 I (the pre-mass-matrix behavior). */ | ||
| JointLevelKFPreFilter(SensorOutputMapReadOnly sensorMap, |
There was a problem hiding this comment.
Because of the way this whole thing is written, a lot of your allocations occur in the allocate() function. This means that the objects themselves aren't declared as final, and they can be overriden. While it makes the constructor smaller, it provides more vulnerability to the codebase. All objects should be final, which means they should be declared eitehr in the constructor or the class definition. In the case of most of these, you could declare them in the class definition and then call reshape in the allocation block, which would satisfy this.
| int ci = massMatrixColumn[i]; | ||
| for (int j = 0; j < n; j++) | ||
| Mff.set(i, j, massMatrix.get(ci, massMatrixColumn[j])); // M_ff (nxn) | ||
| } |
There was a problem hiding this comment.
Is there a reason this isn't using block operations? I see the thing saying you shouldn't assume ordering, but you're literally constructing it, you could construct it such that ordering is enforced. Even if you don't, you could still do column-wise block operations, which are written into CommonOps_DDRM.extract(..) or MatrixTools.setMatrixBlock(...). You could also write a convenience function in MatrixTools.setMatrixColumn that handles some of that for you, or even provide an array of columns to set, which is these for loops, which would make the code more readable, testable, and reliable.
| // Gap-joint reflected rotor inertia on the nuisance-block diagonal (Part B item 1): 0 on the 6 base rows, | ||
| // the joint's reflected rotor inertia on each unfiltered gap joint (none on Alex; kept general). Floors | ||
| // M_NN the same way the filtered rotor diagonal floors Lambda_eff below. | ||
| if (nuisanceRotorInertiaDiag != null) |
There was a problem hiding this comment.
I don't find the naming "nuisance" as intuitive at all. What does that mean?
| // M_NN the same way the filtered rotor diagonal floors Lambda_eff below. | ||
| if (nuisanceRotorInertiaDiag != null) | ||
| for (int a = 0; a < nN; a++) | ||
| MNN.add(a, a, nuisanceRotorInertiaDiag[a]); |
There was a problem hiding this comment.
The name of this object leavea lot to be desired.
| // Lambda = M_ff - M_jN X = M_ff - M_Nf^T X (M symmetric => M_jN = M_Nf^T). SPEC §3.2. | ||
| CommonOps_DDRM.multTransA(MNf, MNNInvMNf, Lambda); // Lambda <- M_Nf^T X | ||
| CommonOps_DDRM.changeSign(Lambda); // Lambda <- -M_jN X | ||
| CommonOps_DDRM.addEquals(Lambda, Mff); // Lambda <- M_ff - M_jN X |
There was a problem hiding this comment.
Replace these three lines with
Lambda.set(Mff);
CommonOps_DDRM.multAddTransA(-1.0, MNf, MNNInvMNf, Lambda);
|
|
||
| public static void set_matrix(DMatrixRMaj out, RotationMatrixReadOnly r) | ||
| { | ||
| out.set(0, 0, r.getM00()); |
There was a problem hiding this comment.
This is the same as saying r.get(out);
| private final double dt; | ||
|
|
||
| private final LinkedHashMap<OneDoFJointBasics, Integer> jointToIndex = new LinkedHashMap<>(); | ||
| private final LinkedHashMap<IMUSensorReadOnly, Integer> imuToOrdinal = new LinkedHashMap<>(); |
There was a problem hiding this comment.
For all this stuff, I would look at how the JointIndexHandler was written, as it does a lot of similar things.
|
|
||
| private static final Vector3D E_X = new Vector3D(1.0, 0.0, 0.0); | ||
| private static final Vector3D E_Y = new Vector3D(0.0, 1.0, 0.0); | ||
| private static final Vector3D E_Z = new Vector3D(0.0, 0.0, 1.0); |
There was a problem hiding this comment.
these can all be replaced by Axis3D.X, Y, Z
| rotation.inverseTransform(E_X, rTransposeEx); | ||
| rotation.inverseTransform(E_Y, rTransposeEy); | ||
| measurementJacobian.reshape(MEASUREMENT_SIZE, tangentSize); | ||
| measurementJacobian.zero(); |
There was a problem hiding this comment.
again, I could be wrong, but I think the zero call is done automatically.
| private final GravityLevelingUpdater gravityUpdater; | ||
|
|
||
| /** |g| (m/s²), used only by the gravity-leveling quasi-static gate. */ | ||
| private static final double GRAVITY_MAGNITUDE = 9.81; |
There was a problem hiding this comment.
We pass this in in the robot model, so that there's a consistent magnitude used throughout the controller.
…InEKF InvariantEKF hard-coded GRAVITY_MAGNITUDE = 9.81 for the gravity-leveling gate and InvariantPropagator hard-coded GRAVITY_Z = -9.81 for the dynamics: two independent literals for one quantity, and the InEKF was the only estimator component not taking gravity as an input (DRCKinematicsBasedStateEstimator, PelvisIMUBasedLinearStateCalculator, IMUBiasStateEstimator and JointLevelKFPreFilter all already do). Both now come from AvatarEstimatorThreadFactory.getGravity(), i.e. from AvatarMultiThreadingFactory.GRAVITY on hardware and SCS2AvatarSimulationFactory's gravity in simulation. Following the existing IHMC convention the sign is not considered: the propagator normalizes to (0, 0, -|g|) and the gate to |g|, so one number configures both and they cannot drift apart. StateEstimatorControllerFactory.createStateEstimator also takes it, so secondary estimators are handed the same gravity as the main one rather than carrying their own copy. Behaviour is unchanged at the -9.81 every call site already used. InvariantEstimatorGravityInjectionTest pins the two properties a re-hard-coded constant would break, at lunar gravity: free fall integrates the injected |g|, and the quasi-static gate opens at the injected |g| while rejecting 9.81. Estimator suite: 178 tests / 29 classes green.
Put gravity as `AvatarMultiThreadFactory` instead of in the estimator as a hard-coded constant.
3 participants
List of Things to Change/Fix
Priority 1: Structural
JointLevelKFPreFilter.javainto more organized componentsPriority 2: Realtime Ops / Legibility
IntegerversusTIntegeruses (viaTroveJava library)Priority 3: Quality of Life Stuff