/** * 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); } }
@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 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())); }
RevisedBacklashCompensatingVelocityYoVariable revisedBacklashCompensatingVelocity = new RevisedBacklashCompensatingVelocityYoVariable("bl_qd_velocity2", "", alphaVariable, backlashyPosition, dt, slopTime, registry);
RevisedBacklashCompensatingVelocityYoVariable revisedBacklashCompensatingVelocity = new RevisedBacklashCompensatingVelocityYoVariable("bl_qd_velocity2", "", alphaVariable, noisyPosition, dt, slopTime, registry);
RevisedBacklashCompensatingVelocityYoVariable revisedBacklashCompensatingVelocity = new RevisedBacklashCompensatingVelocityYoVariable("bl_qd_velocity2", "", alphaVariable, backlashyPosition, dt, slopTime, registry);