public void startTransition(double lengthOfTransitionTime) { transitionStartTime.set(time.getDoubleValue()); this.lengthOfTransitionTime = lengthOfTransitionTime; }
protected AbstractPDController(String suffix, YoVariableRegistry registry) { positionError = new YoDouble("positionError_" + suffix, registry); positionError.set(0.0); rateError = new YoDouble("rateError_" + suffix, registry); rateError.set(0.0); actionP = new YoDouble("action_P_" + suffix, registry); actionP.set(0.0); actionD = new YoDouble("action_D_" + suffix, registry); actionD.set(0.0); }
public void update(double input) { if (!hasBeenInitialized.getBooleanValue()) initialize(input); double positionError = input - this.getDoubleValue(); double acceleration = -velocityGain.getDoubleValue() * smoothedRate.getDoubleValue() + positionGain.getDoubleValue() * positionError; acceleration = MathTools.clamp(acceleration, -maximumAcceleration.getDoubleValue(), maximumAcceleration.getDoubleValue()); smoothedAcceleration.set(acceleration); smoothedRate.add(smoothedAcceleration.getDoubleValue() * dt); smoothedRate.set(MathTools.clamp(smoothedRate.getDoubleValue(), maximumRate.getDoubleValue())); this.add(smoothedRate.getDoubleValue() * dt); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testDefaultMaximumDeflection4() { Random random = new Random(1561651L); String name = "compensator"; YoVariableRegistry registry = new YoVariableRegistry(""); YoDouble stiffness = new YoDouble("stiffness", registry); YoDouble maximumDeflection = new YoDouble("maximumDeflection", registry); YoDouble rawJointPosition = new YoDouble("rawJointPosition", registry); YoDouble jointTau = new YoDouble("jointTau", registry); ElasticityCompensatorYoVariable elasticityCompensatorYoVariable = new ElasticityCompensatorYoVariable(name, stiffness, maximumDeflection, rawJointPosition, jointTau, registry); maximumDeflection.set(0.1); for (int i = 0; i < 10000; i++) { stiffness.set(RandomNumbers.nextDouble(random, 1.0, 100000.0)); rawJointPosition.set(RandomNumbers.nextDouble(random, 100.0)); jointTau.set(RandomNumbers.nextDouble(random, 10000.0)); elasticityCompensatorYoVariable.update(); double deflectedJointPosition = rawJointPosition.getDoubleValue() - MathTools.clamp(jointTau.getDoubleValue() / stiffness.getDoubleValue(), maximumDeflection.getValue()); assertEquals(deflectedJointPosition, elasticityCompensatorYoVariable.getDoubleValue(), EPSILON); } }
@Override public void doControl() { desiredLidarAngle.add(desiredLidarVelocity.getDoubleValue() * controlDT); double lidarJointTau = lidarJointController.compute(lidarJoint.getQYoVariable().getDoubleValue(), desiredLidarAngle.getDoubleValue(), lidarJoint.getQDYoVariable() .getDoubleValue(), desiredLidarVelocity.getDoubleValue(), controlDT) + lidarJoint.getDamping() * desiredLidarVelocity.getDoubleValue(); lidarJoint.setTau(lidarJointTau); }
public DitherProducer(String namePrefix, YoVariableRegistry parentRegistry, double controlDT) { registry = new YoVariableRegistry(namePrefix); desiredDitherFrequency = new YoDouble(namePrefix + "_dither_frequency_desired", registry); desiredDitherFrequency.set(0.0); desiredDitherFrequency.addVariableChangedListener(new VariableChangedListener() { @Override public void notifyOfVariableChange(YoVariable<?> v) { checkFrequency(); } }); ditherFrequency = new YoDouble(namePrefix + "_dither_frequency", registry); ditherFrequency.set(0.0); amplitude = new YoDouble(namePrefix + "_dither_amplitude", registry); amplitude.set(0.0); dither = new YoDouble(namePrefix + "_dither_output", registry); dither.set(0.0); maxFrequency = 1.0 / (2.0 * controlDT); if (parentRegistry != null) { parentRegistry.addChild(registry); } }
@Override public Artifact createArtifact() { MutableColor color3f = appearance.getColor(); YoDouble endPointX = new YoDouble(getName() + "ArtifactEndPointX", base.getYoX().getYoVariableRegistry()); YoDouble endPointY = new YoDouble(getName() + "ArtifactEndPointY", base.getYoY().getYoVariableRegistry()); base.getYoX().addVariableChangedListener(v -> endPointX.set(base.getX() + vector.getX())); base.getYoY().addVariableChangedListener(v -> endPointY.set(base.getY() + vector.getY())); vector.getYoX().addVariableChangedListener(v -> endPointX.set(base.getX() + vector.getX())); vector.getYoY().addVariableChangedListener(v -> endPointY.set(base.getY() + vector.getY())); return new YoArtifactLineSegment2d(getName(), new YoFrameLineSegment2D(base.getYoX(), base.getYoY(), endPointX, endPointY, ReferenceFrame.getWorldFrame()), color3f.get()); }
private void sendFootstepPlan() { FootstepPlanningToolboxOutputStatus plannerResult = this.plannerResult.getAndSet(null); FootstepDataListMessage footstepDataListMessage = plannerResult.getFootstepDataList(); footstepDataListMessage.setDefaultSwingDuration(swingTime.getValue()); footstepDataListMessage.setDefaultTransferDuration(transferTime.getDoubleValue()); footstepDataListMessage.setDestination(PacketDestination.CONTROLLER.ordinal()); footstepPublisher.publish(footstepDataListMessage); }
@Override public YoDouble duplicate(YoVariableRegistry newRegistry) { DoubleParameter newParameter = new DoubleParameter(getName(), getDescription(), newRegistry, initialValue, getManualScalingMin(), getManualScalingMax()); newParameter.value.set(value.getValue()); newParameter.loadStatus = getLoadStatus(); return newParameter.value; } }
public AcsellActuatorCommand(String name, AcsellActuator actuator, YoVariableRegistry parentRegistry) { this.actuator = actuator; this.currentLimit = actuator.getCurrentLimit(); this.registry = new YoVariableRegistry(name); this.enabled = new YoBoolean(name + "Enabled", registry); this.tauDesired = new YoDouble(name + "TauDesired", registry); this.tauInertia = new YoDouble(name + "TauInertia", registry); this.qddDesired = new YoDouble(name + "qdd_d", registry); this.damping = new YoDouble(name + "Damping", registry); //this.currentDesired = new YoDouble(name+"CurrentDesired", registry); this.rawCurrentDesired = new YoDouble(name+"CurrentDesired", registry); if(actuator==StepprActuator.LEFT_HIP_Z || actuator==StepprActuator.RIGHT_HIP_Z) this.filteredCurrentDesired = new AlphaFilteredYoVariable(name+"CurrentDesired_filt", registry, 0.90, rawCurrentDesired); else this.filteredCurrentDesired = new AlphaFilteredYoVariable(name+"CurrentDesired_filt", registry, 0.0, rawCurrentDesired); rawCurrentDesired.addVariableChangedListener(new VariableChangedListener() { @Override public void notifyOfVariableChange(YoVariable<?> v) { if(v.getValueAsDouble()>currentLimit) v.setValueFromDouble(currentLimit); if(v.getValueAsDouble()<-currentLimit) v.setValueFromDouble(-currentLimit); } }); parentRegistry.addChild(registry); }
joystickUpdater.addJoystickEventListener(new DoubleYoVariableJoystickEventListener(desiredVelX_Adjust, joystickUpdater.findComponent(Component.Identifier.Axis.Y), -maxDesiredVelocityX_Adjust, maxDesiredVelocityX_Adjust, deadZone, false)); desiredVelX_Adjust.addVariableChangedListener(new VariableChangedListener() desiredVelX_Setpoint.addVariableChangedListener(new VariableChangedListener() desiredVelocityX.addVariableChangedListener(new VariableChangedListener() desiredVelocityY.set(desiredVelocityY_Bias); joystickUpdater.addJoystickEventListener(new DoubleYoVariableJoystickEventListener(desiredVelocityY, joystickUpdater.findComponent(Component.Identifier.Axis.X), -0.1+desiredVelocityY_Bias, 0.1+desiredVelocityY_Bias, deadZone, false)); desiredHeadingDot.set(desiredHeadingDot_Bias); joystickUpdater.addJoystickEventListener(new DoubleYoVariableJoystickEventListener(desiredHeadingDot, joystickUpdater.findComponent(Component.Identifier.Axis.RZ), -0.1+desiredHeadingDot_Bias, 0.1+desiredHeadingDot_Bias, deadZone/2.0, true));
public YoAverager(String prefix, YoVariableRegistry registry) { average = new YoDouble(prefix + "Average", registry); nUpdates = new YoInteger(prefix + "AverageNUpdates", registry); }
public YoDoubleStatistics(YoDouble variable, YoVariableRegistry registry) { this.variable = variable; this.registry = registry; variable.addVariableChangedListener(this::update); }
@Override public double getProportionalGain() { return proportionalGain.getValue(); }
@Override public void compute(double time) { integratedPhaseAngle.add(2.0 * Math.PI * frequency.getValue() * controlDT); double angle = integratedPhaseAngle.getValue(); double mult = 2.0 * Math.PI * frequency.getValue(); double alpha = 0.5 * Math.sin(angle) + 0.5; double alphaDot = 0.5 * 2.0 * mult * Math.cos(angle); double alphaDDot = -0.5 * 2.0 * 2.0 * mult * mult * Math.sin(angle); limitedPointA.update(); limitedPointB.update(); position.interpolate(limitedPointA, limitedPointB, alpha); linearVelocity.sub(limitedPointB, limitedPointA); linearVelocity.scale(alphaDot); linearAcceleration.sub(limitedPointB, limitedPointA); linearAcceleration.scale(alphaDDot); }
public Matrix3D createLinearAccelerationWeightMatrix() { Matrix3D weightMatrix = new Matrix3D(); xAccelerationWeights.addVariableChangedListener(new MatrixUpdater(0, 0, weightMatrix)); yAccelerationWeights.addVariableChangedListener(new MatrixUpdater(1, 1, weightMatrix)); zAccelerationWeight.addVariableChangedListener(new MatrixUpdater(2, 2, weightMatrix)); xAccelerationWeights.notifyVariableChangedListeners(); yAccelerationWeights.notifyVariableChangedListeners(); zAccelerationWeight.notifyVariableChangedListeners(); return weightMatrix; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRandomMaximumDeflection4() { Random random = new Random(1561651L); String name = "compensator"; YoVariableRegistry registry = new YoVariableRegistry(""); YoDouble stiffness = new YoDouble("stiffness", registry); YoDouble maximumDeflection = new YoDouble("maximumDeflection", registry); YoDouble rawJointPosition = new YoDouble("rawJointPosition", registry); YoDouble jointTau = new YoDouble("jointTau", registry); ElasticityCompensatorYoVariable elasticityCompensatorYoVariable = new ElasticityCompensatorYoVariable(name, stiffness, maximumDeflection, rawJointPosition, jointTau, registry); maximumDeflection.set(0.1); for (int i = 0; i < 10000; i++) { stiffness.set(RandomNumbers.nextDouble(random, 1.0, 100000.0)); rawJointPosition.set(RandomNumbers.nextDouble(random, 100.0)); jointTau.set(RandomNumbers.nextDouble(random, 10000.0)); maximumDeflection.set(RandomNumbers.nextDouble(random, 0.0, 10.0)); elasticityCompensatorYoVariable.update(); double deflectedJointPosition = rawJointPosition.getDoubleValue() - MathTools.clamp(jointTau.getDoubleValue() / stiffness.getDoubleValue(), maximumDeflection.getValue()); assertEquals(deflectedJointPosition, elasticityCompensatorYoVariable.getDoubleValue(), EPSILON); } } }
public void computeNextTick(FramePoint3D positionToPack, double deltaT) { timeIntoStep.add(deltaT); compute(timeIntoStep.getDoubleValue()); getPosition(positionToPack); }