/** * Get the initialization state of this toolbox controller: * <ul> * <li>{@code true}: this toolbox controller has been initialized properly and is ready for doing * some computation! * <li>{@code false}: this toolbox controller has either not been initialized yet or the * initialization process failed. * </ul> * * @return the initialization state of this toolbox controller. */ public boolean hasBeenInitialized() { return !initialize.getValue(); }
@Override public boolean test(DoubleProvider encoderDataHolder) { if (forceMotorBasedPositionSwitch.getValue()) { deadCount = 100; } else if (encoderDataHolder.getValue() == lastValue) { if (deadCount < threshold) deadCount++; } else { deadCount = 0; } lastValue = encoderDataHolder.getValue(); isFingerJointEncoderDead.set(deadCount >= threshold); return isFingerJointEncoderDead.getValue(); } };
@Override public void doControl() { if(!this.isInitialized.getValue()) { robotController.initialize(); this.isInitialized.set(true); } robotController.doControl(); }
@Override public YoBoolean duplicate(YoVariableRegistry newRegistry) { BooleanParameter newParameter = new BooleanParameter(getName(), getDescription(), newRegistry, initialValue); newParameter.value.set(value.getValue()); newParameter.loadStatus = getLoadStatus(); return newParameter.value; } }
public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) { if (!hasBeenCalled.getValue()) { hasBeenCalled.set(true); set(xUnfiltered, yUnfiltered, zUnfiltered); } else { unfilteredPoint3D.set(xUnfiltered, yUnfiltered, zUnfiltered); interpolate(unfilteredPoint3D, this, alphaProvider.getValue()); } } }
public void update(double xUnfiltered, double yUnfiltered) { if (!hasBeenCalled.getValue()) { hasBeenCalled.set(true); set(xUnfiltered, yUnfiltered); } else { unfilteredPoint2D.set(xUnfiltered, yUnfiltered); interpolate(unfilteredPoint2D, this, alphaProvider.getValue()); } } }
public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) { if (!hasBeenCalled.getValue()) { hasBeenCalled.set(true); set(xUnfiltered, yUnfiltered, zUnfiltered); } else { unfilteredVector3D.set(xUnfiltered, yUnfiltered, zUnfiltered); interpolate(unfilteredVector3D, this, alphaProvider.getValue()); } } }
public void update(double xUnfiltered, double yUnfiltered) { if (!hasBeenCalled.getValue()) { hasBeenCalled.set(true); set(xUnfiltered, yUnfiltered); } else { unfilteredVector2D.set(xUnfiltered, yUnfiltered); interpolate(unfilteredVector2D, this, alphaProvider.getValue()); } } }
public void update(Vector3DReadOnly inputAngularVelocity, Vector3DReadOnly inputLinearAcceleration, Vector3DReadOnly inputMagneticVector) { if (!hasBeenInitialized.getValue()) { initialize(inputLinearAcceleration, inputMagneticVector); return; } boolean success = computeOrientationError((QuaternionReadOnly) estimatedOrientation, inputLinearAcceleration, inputMagneticVector, quaternionUpdate); if (success) { quaternionUpdate.getRotationVector(totalError); yoErrorTerm.set(totalError); integralTerm.scaleAdd(integralGain.getValue() * updateDT, yoErrorTerm, yoIntegralTerm); yoIntegralTerm.set(integralTerm); angularVelocityTerm.scaleAdd(proportionalGain.getValue(), totalError, inputAngularVelocity); angularVelocityTerm.add(integralTerm); } else { yoErrorTerm.setToZero(); angularVelocityTerm.set(inputAngularVelocity); } rotationUpdate.setAndScale(updateDT, angularVelocityTerm); quaternionUpdate.setRotationVector(rotationUpdate); estimatedOrientation.multiply(quaternionUpdate); if (estimatedAngularVelocity != null) estimatedAngularVelocity.add(inputAngularVelocity, integralTerm); }
submitHandPose(robotSide, desiredUpperArmOrientation, -Math.PI / 2.0, null, mirrorOrientationsForRightSide); if (canArmsReachFarBehind.getValue()) desiredUpperArmOrientation.setYawPitchRoll(1.2, 0.90 * Math.PI / 2.0, 0.0); // Normal Running man else
submitFootstepPose(true, robotSide, desiredFootstepPosition); if (canArmsReachFarBehind.getValue()) desiredUpperArmOrientation.setYawPitchRoll(0.2, -0.05, -1.3708); else pipeLine.requestNewStage(); if (canArmsReachFarBehind.getValue()) if (canArmsReachFarBehind.getValue()) pipeLine.requestNewStage(); if (canArmsReachFarBehind.getValue())
comInitialVelocityList, comFinalVelocityList, comInitialAccelerationList, comFinalAccelerationList, copTrajectoryGenerator.getNumberOfFootstepsRegistered()); angularMomentumGenerator.computeReferenceAngularMomentumStartingFromDoubleSupport(isInitialTransfer.getBooleanValue(), isStanding.getValue());
comInitialVelocityList, comFinalVelocityList, comInitialAccelerationList, comFinalAccelerationList, copTrajectoryGenerator.getNumberOfFootstepsRegistered()); angularMomentumGenerator.computeReferenceAngularMomentumStartingFromDoubleSupport(isInitialTransfer.getBooleanValue(), isStanding.getValue()); angularMomentumGenerator.initializeForDoubleSupport(0.0, isStanding.getBooleanValue());