@Override public void update() { if (position == null) { throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " + "position variable to call update(), otherwise use update(double)"); } update(position.getDoubleValue()); }
/** * Compute the joint velocities (for a specific subset of joints) by calculating finite-difference on joint positions and applying a backlash compensator (see {@link RevisedBacklashCompensatingVelocityYoVariable}). It is then automatically low-pass filtered. * This is not cumulative and has the effect of ignoring the velocity signal provided by the robot. * @param alphaFilter low-pass filter parameter. * @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 computeJointVelocityWithBacklashCompensatorWithJointsToIgnore(DoubleYoVariable alphaFilter, 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 intermediateJointPosition = outputJointPositions.get(oneDoFJoint); List<ProcessingYoVariable> processors = processedJointVelocities.get(oneDoFJoint); String prefix = JOINT_VELOCITY.getProcessorNamePrefix(BACKLASH); String suffix = JOINT_VELOCITY.getProcessorNameSuffix(jointName, processors.size()); RevisedBacklashCompensatingVelocityYoVariable jointVelocity = new RevisedBacklashCompensatingVelocityYoVariable(prefix + suffix, "", alphaFilter, intermediateJointPosition, updateDT, slopTime, registry); processors.add(jointVelocity); if (!forVizOnly) outputJointVelocities.put(oneDoFJoint, jointVelocity); } }
private void updateUsingDifference(double difference) { double previousFilteredDerivative = getDoubleValue(); double currentRawDerivative = difference / dt; double alpha = alphaVariable.getValue(); this.set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); }
@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); } }
set(0.0); difference = scaleFactor * difference; this.updateUsingDifference(difference); lastPosition.set(currentPosition);
public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, DoubleProvider alphaVariable, YoDouble positionVariable, double dt, DoubleProvider slopTime, YoVariableRegistry registry) { super(name, description, registry); finiteDifferenceVelocity = new FilteredVelocityYoVariable(name + "finiteDifferenceVelocity", "", alphaVariable, dt, 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); position = positionVariable; this.alphaVariable = alphaVariable; this.slopTime = slopTime; this.dt = dt; lastPosition = new YoDouble(name + "_lastPosition", registry); reset(); }
RevisedBacklashCompensatingVelocityYoVariable revisedBacklashCompensatingVelocity = new RevisedBacklashCompensatingVelocityYoVariable("bl_qd_velocity2", "", alphaVariable, backlashyPosition, dt, slopTime, registry); revisedBacklashCompensatingVelocity.update(); assertEquals(0.0, revisedBacklashCompensatingVelocity.getDoubleValue(), 1e-3);
set(0.0); difference = scaleFactor * difference; this.updateUsingDifference(difference); lastPosition.set(currentPosition);
public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, YoDouble alphaVariable, double dt, YoDouble slopTime, YoVariableRegistry registry) { super(name, description, registry); finiteDifferenceVelocity = new FilteredVelocityYoVariable(name + "finiteDifferenceVelocity", "", alphaVariable, dt, 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 + "timeInState", registry); this.position = null; this.alphaVariable = alphaVariable; this.slopTime = slopTime; this.dt = dt; lastPosition = new YoDouble(name + "_lastPosition", registry); reset(); }
RevisedBacklashCompensatingVelocityYoVariable revisedBacklashCompensatingVelocity = new RevisedBacklashCompensatingVelocityYoVariable("bl_qd_velocity2", "", alphaVariable, noisyPosition, dt, slopTime, registry); revisedBacklashCompensatingVelocity.update(); reconstructedPosition2.add(revisedBacklashCompensatingVelocity.getDoubleValue() * dt);
public void update() { if (position == null) { throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " + "position variable to call update(), otherwise use update(double)"); } update(position.getDoubleValue()); }
private void updateUsingDifference(double difference) { double previousFilteredDerivative = getDoubleValue(); double currentRawDerivative = difference / dt; double alpha = alphaVariable.getDoubleValue(); this.set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); }
public ValkyrieSliderBoardJointHolder(ValkyrieRosControlSliderBoard valkyrieRosControlSliderBoard, OneDoFJointBasics joint, YoVariableRegistry parentRegistry, double dt) { this.valkyrieRosControlSliderBoard = valkyrieRosControlSliderBoard; this.joint = joint; this.dt = dt; String jointName = joint.getName(); this.registry = new YoVariableRegistry(jointName); this.pdController = new PDController(jointName, registry); pdController.setProportionalGain(ValkyrieRosControlSliderBoard.KP_DEFAULT); pdController.setDerivativeGain(ValkyrieRosControlSliderBoard.KD_DEFAULT); q = new YoDouble(jointName + "_q", registry); qd = new YoDouble(jointName + "_qd", registry); bl_qd = new RevisedBacklashCompensatingVelocityYoVariable("bl_qd_" + jointName, "", valkyrieRosControlSliderBoard.jointVelocityAlphaFilter, q, dt, valkyrieRosControlSliderBoard.jointVelocitySlopTime, registry); tau = new YoDouble(jointName + "_tau", registry); q_d = new YoDouble(jointName + "_q_d", registry); qd_d = new YoDouble(jointName + "_qd_d", registry); if (valkyrieRosControlSliderBoard.setPointMap != null && valkyrieRosControlSliderBoard.setPointMap.containsKey(jointName)) q_d.set(valkyrieRosControlSliderBoard.setPointMap.get(jointName)); tau_offset = new YoDouble(joint.getName() + "_tau_offset", parentRegistry); tau_d = new YoDouble(joint.getName() + "_tau_d", registry); jointCommand_pd = new YoDouble(joint.getName() + "_tau_pd", registry); jointCommand_function = new YoDouble(joint.getName() + "_tau_function", registry); if (valkyrieRosControlSliderBoard.torqueOffsetMap != null && valkyrieRosControlSliderBoard.torqueOffsetMap.containsKey(joint.getName())) tau_offset.set(-valkyrieRosControlSliderBoard.torqueOffsetMap.get(joint.getName())); }
public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, DoubleYoVariable alphaVariable, DoubleYoVariable positionVariable, double dt, DoubleYoVariable slopTime, YoVariableRegistry registry) { super(name, description, registry); finiteDifferenceVelocity = new FilteredVelocityYoVariable(name + "finiteDifferenceVelocity", "", alphaVariable, dt, 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); position = positionVariable; this.alphaVariable = alphaVariable; this.slopTime = slopTime; this.dt = dt; lastPosition = new DoubleYoVariable(name + "_lastPosition", registry); reset(); }
RevisedBacklashCompensatingVelocityYoVariable revisedBacklashCompensatingVelocity = new RevisedBacklashCompensatingVelocityYoVariable("bl_qd_velocity2", "", alphaVariable, backlashyPosition, dt, slopTime, registry); revisedBacklashCompensatingVelocity.update(); reconstructedPosition2.add(revisedBacklashCompensatingVelocity.getDoubleValue() * dt);
@Override public void update() { joint.setQ(handle.getPosition()); joint.setQd(handle.getVelocity()); bl_qd.update(); joint.setTau(handle.getEffort()); q.set(joint.getQ()); qd.set(joint.getQd()); tau.set(joint.getTau()); positionStepSizeLimiter.updateOutput(q.getDoubleValue(), q_d.getDoubleValue()); handle.setDesiredPosition(positionStepSizeLimiter.getDoubleValue()); } }
public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, DoubleYoVariable alphaVariable, double dt, DoubleYoVariable slopTime, YoVariableRegistry registry) { super(name, description, registry); finiteDifferenceVelocity = new FilteredVelocityYoVariable(name + "finiteDifferenceVelocity", "", alphaVariable, dt, 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 + "timeInState", registry); this.position = null; this.alphaVariable = alphaVariable; this.slopTime = slopTime; this.dt = dt; lastPosition = new DoubleYoVariable(name + "_lastPosition", registry); reset(); }
@Override public void update() { joint.setQ(handle.getPosition()); joint.setQd(handle.getVelocity()); bl_qd.update(); joint.setTau(handle.getEffort()); q.set(joint.getQ()); qd.set(joint.getQd()); tau.set(joint.getTau()); double pdOutput = pdController.compute(q.getDoubleValue(), q_d.getDoubleValue(), qd.getDoubleValue(), qd_d.getDoubleValue()); jointCommand_pd.set(pdOutput); tau_d.set(valkyrieRosControlSliderBoard.masterScaleFactor.getDoubleValue() * (jointCommand_pd.getDoubleValue() + jointCommand_function.getDoubleValue()) + tau_offset .getDoubleValue()); handle.setDesiredEffort(tau_d.getDoubleValue()); } }