@Override public void update() { if (velocity == null) { throw new NullPointerException( "BacklashProcessingYoVariable must be constructed with a non null " + "velocity variable to call update(), otherwise use update(double)"); } update(velocity.getDoubleValue()); }
public void update() { if (velocity == null) { throw new NullPointerException( "BacklashProcessingYoVariable must be constructed with a non null " + "velocity variable to call update(), otherwise use update(double)"); } update(velocity.getDoubleValue()); }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testAgainstRevisedBacklash() { YoVariableRegistry registry = new YoVariableRegistry("dummy"); YoDouble slopTime = new YoDouble("slopTime", registry); double dt = 0.002; YoDouble alpha = new YoDouble("alpha", registry); alpha.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(16.0, dt)); YoDouble positionVariable = new YoDouble("rawPosition", registry); FilteredVelocityYoVariable velocityVariable = new FilteredVelocityYoVariable("fd", "", alpha, positionVariable, dt, registry); BacklashProcessingYoVariable blToTest = new BacklashProcessingYoVariable("blTest", "", velocityVariable, dt, slopTime, registry); RevisedBacklashCompensatingVelocityYoVariable blExpected = new RevisedBacklashCompensatingVelocityYoVariable("blExpected", "", alpha, positionVariable, dt, slopTime, registry); Random random = new Random(561651L); for (double t = 0.0; t < 100.0; t += dt) { positionVariable.set(2.0 * Math.sin(2.0 * Math.PI * 10.0) + RandomNumbers.nextDouble(random, 1.0) * Math.sin(2.0 * Math.PI * 30.0 + 2.0 / 3.0 * Math.PI)); velocityVariable.update(); blToTest.update(); blExpected.update(); assertEquals(blToTest.getDoubleValue(), blExpected.getDoubleValue(), 1.0e-10); } }
public void compute() { jacobian.compute(); CommonOps.extract(jacobian.getJacobianMatrix(), 0, 3, 0, 3, jacobianAngularPart64F, 0, 0); if (Math.abs(CommonOps.det(jacobianAngularPart64F)) < 1e-5) return; CommonOps.invert(jacobianAngularPart64F, inverseAngularJacobian64F); chestAngularVelocity.setToZero(chestIMU.getMeasurementFrame()); chestIMU.getAngularVelocityMeasurement(chestAngularVelocity.getVector()); chestAngularVelocity.changeFrame(jacobian.getJacobianFrame()); pelvisAngularVelocity.setToZero(pelvisIMU.getMeasurementFrame()); pelvisIMU.getAngularVelocityMeasurement(pelvisAngularVelocity.getVector()); pelvisAngularVelocity.changeFrame(jacobian.getJacobianFrame()); chestAngularVelocity.sub(pelvisAngularVelocity); omega.setData(chestAngularVelocity.toArray()); CommonOps.mult(inverseAngularJacobian64F, omega, qd_estimated); for (int i = 0; i < joints.length; i++) { OneDoFJoint joint = joints[i]; double qd_sensorMap = sensorMap.getJointVelocityProcessedOutput(joint); double qd_IMU = qd_estimated.get(i, 0); double qd_fused = (1.0 - alpha.getDoubleValue()) * qd_sensorMap + alpha.getDoubleValue() * qd_IMU; jointVelocitiesFromIMUOnly.get(joint).set(qd_IMU); jointVelocities.get(joint).update(qd_fused); } }