public double getEstimatedJointVelocitiy(OneDoFJoint joint) { BacklashProcessingYoVariable estimatedJointVelocity = jointVelocities.get(joint); if (estimatedJointVelocity != null) return estimatedJointVelocity.getDoubleValue(); else return Double.NaN; } }
@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); } }
/** * 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)); } }
@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 BacklashProcessingYoVariable(String name, String description, YoDouble velocityVariable, double dt, DoubleProvider slopTime, YoVariableRegistry registry) { super(name, description, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); backlashState = new YoEnum<BacklashState>(name + "BacklashState", registry, BacklashState.class, true); backlashState.set(null); timeSinceSloppy = new YoDouble(name + "TimeSinceSloppy", registry); velocity = velocityVariable; this.slopTime = slopTime; this.dt = dt; reset(); }
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 BacklashProcessingYoVariable(String name, String description, DoubleYoVariable velocityVariable, double dt, DoubleYoVariable slopTime, YoVariableRegistry registry) { super(name, description, registry); this.hasBeenCalled = new BooleanYoVariable(name + "HasBeenCalled", registry); backlashState = new EnumYoVariable<BacklashState>(name + "BacklashState", registry, BacklashState.class, true); backlashState.set(null); timeSinceSloppy = new DoubleYoVariable(name + "TimeSinceSloppy", registry); velocity = velocityVariable; this.slopTime = slopTime; this.dt = dt; reset(); }
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); } }