/** * Apply a backlash compensator (see {@link BacklashProcessingYoVariable}) to the joint velocity. * Useful when the robot has backlash in its joints or simply to calm down small shakies when the robot is at rest. * Implemented as a cumulative processor but should probably be called only once. * @param slopTime every time the velocity changes sign, a slop is engaged during which a confidence factor is ramped up from 0 to 1. * @param forVizOnly if set to true, the result will not be used as the input of the next processing stage, nor as the output of the sensor processing. * @param jointsToIgnore list of the names of the joints to ignore. */ public void addJointVelocityBacklashFilterWithJointsToIgnore(DoubleYoVariable slopTime, boolean forVizOnly, String... jointsToIgnore) { List<String> jointToIgnoreList = new ArrayList<>(); if (jointsToIgnore != null && jointsToIgnore.length > 0) jointToIgnoreList.addAll(Arrays.asList(jointsToIgnore)); for (int i = 0; i < jointSensorDefinitions.size(); i++) { OneDoFJoint oneDoFJoint = jointSensorDefinitions.get(i); String jointName = oneDoFJoint.getName(); if (jointToIgnoreList.contains(jointName)) continue; DoubleYoVariable intermediateJointVelocity = outputJointVelocities.get(oneDoFJoint); List<ProcessingYoVariable> processors = processedJointVelocities.get(oneDoFJoint); String prefix = JOINT_VELOCITY.getProcessorNamePrefix(BACKLASH); String suffix = JOINT_VELOCITY.getProcessorNameSuffix(jointName, processors.size()); BacklashProcessingYoVariable filteredJointVelocity = new BacklashProcessingYoVariable(prefix + suffix, "", intermediateJointVelocity, updateDT, slopTime, registry); processors.add(filteredJointVelocity); if (!forVizOnly) outputJointVelocities.put(oneDoFJoint, filteredJointVelocity); } }
public static BacklashProcessingYoFrameVector createBacklashProcessingYoFrameVector(String namePrefix, String nameSuffix, double dt, DoubleYoVariable slopTime, YoVariableRegistry registry, YoFrameTuple<?, ?> yoFrameTupleToProcess) { String xName = YoFrameVariableNameTools.createXName(namePrefix, nameSuffix); String yName = YoFrameVariableNameTools.createYName(namePrefix, nameSuffix); String zName = YoFrameVariableNameTools.createZName(namePrefix, nameSuffix); DoubleYoVariable xRaw = yoFrameTupleToProcess.getYoX(); DoubleYoVariable yRaw = yoFrameTupleToProcess.getYoY(); DoubleYoVariable zRaw = yoFrameTupleToProcess.getYoZ(); BacklashProcessingYoVariable x = new BacklashProcessingYoVariable(xName, "", xRaw, dt, slopTime, registry); BacklashProcessingYoVariable y = new BacklashProcessingYoVariable(yName, "", yRaw, dt, slopTime, registry); BacklashProcessingYoVariable z = new BacklashProcessingYoVariable(zName, "", zRaw, dt, slopTime, registry); ReferenceFrame referenceFrame = yoFrameTupleToProcess.getReferenceFrame(); return new BacklashProcessingYoFrameVector(x, y, z, registry, referenceFrame); }
public static BacklashProcessingYoFrameVector createBacklashProcessingYoFrameVector(String namePrefix, String nameSuffix, double dt, DoubleProvider slopTime, YoVariableRegistry registry, YoFrameTuple3D yoFrameTupleToProcess) { String xName = YoFrameVariableNameTools.createXName(namePrefix, nameSuffix); String yName = YoFrameVariableNameTools.createYName(namePrefix, nameSuffix); String zName = YoFrameVariableNameTools.createZName(namePrefix, nameSuffix); YoDouble xRaw = yoFrameTupleToProcess.getYoX(); YoDouble yRaw = yoFrameTupleToProcess.getYoY(); YoDouble zRaw = yoFrameTupleToProcess.getYoZ(); BacklashProcessingYoVariable x = new BacklashProcessingYoVariable(xName, "", xRaw, dt, slopTime, registry); BacklashProcessingYoVariable y = new BacklashProcessingYoVariable(yName, "", yRaw, dt, slopTime, registry); BacklashProcessingYoVariable z = new BacklashProcessingYoVariable(zName, "", zRaw, dt, slopTime, registry); ReferenceFrame referenceFrame = yoFrameTupleToProcess.getReferenceFrame(); return new BacklashProcessingYoFrameVector(x, y, z, registry, referenceFrame); }
public IMUBasedJointVelocityEstimator(IMUSensorReadOnly pelvisIMU, IMUSensorReadOnly chestIMU, SensorOutputMapReadOnly sensorMap, double estimatorDT, double slopTime, YoVariableRegistry registry) { this.sensorMap = sensorMap; this.pelvisIMU = pelvisIMU; this.chestIMU = chestIMU; jacobian = new GeometricJacobian(pelvisIMU.getMeasurementLink(), chestIMU.getMeasurementLink(), chestIMU.getMeasurementLink().getBodyFixedFrame()); joints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.class); String namePrefix = "imuBasedJointVelocityEstimator"; alpha = new DoubleYoVariable(namePrefix + "AlphaFuse", registry); alpha.set(0.0); double dt = estimatorDT; this.slopTime = new DoubleYoVariable(namePrefix + "SlopTime", registry); this.slopTime.set(slopTime); for (OneDoFJoint joint : joints) { jointVelocitiesFromIMUOnly.put(joint, new DoubleYoVariable("qd_" + joint.getName() + "_IMUBased", registry)); jointVelocities.put(joint, new BacklashProcessingYoVariable("qd_" + joint.getName() + "_FusedWithIMU", "", dt, this.slopTime, registry)); } }
@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); } }