public boolean isLoading() //TODO: This only works with compression springs... { return s_filt.getDoubleValue() > 0.5; }
public double getSensorZForceLowPassFilteredInWorld() { return lowPassSensorForceZ.getDoubleValue(); }
@Override public void update(double currentLength) { x = currentLength*springProperties.getDirectionallity(); s.set(Math.signum(last_x-x)*0.5+0.5); s_filt.update(); F = isSpringEngaged() ? -k_L*(x-x0_L)*s_filt.getDoubleValue() -k_U*(x-x0_U)*(1-s_filt.getDoubleValue()) : 0; last_x = x; }
@Override public void doControl() { for(OneDegreeOfFreedomJoint simulatedJoint : elasticJoints) { double tau = simulatedJoint.getTau(); DoubleYoVariable stiffness = jointStiffness.get(simulatedJoint); DoubleYoVariable maxDeflection = maxDeflections.get(simulatedJoint); AlphaFilteredYoVariable filteredDesired = filteredDesiredJointAngles.get(simulatedJoint); double qDesired = -MathTools.clipToMinMax(tau / stiffness.getDoubleValue(), maxDeflection.getDoubleValue()); filteredDesired.update(qDesired); simulatedJoint.setQ(filteredDesired.getDoubleValue()); } }
@Override public void doControl() { for(OneDegreeOfFreedomJoint simulatedJoint : elasticJoints) { double tau = simulatedJoint.getTau(); YoDouble stiffness = jointStiffness.get(simulatedJoint); YoDouble maxDeflection = maxDeflections.get(simulatedJoint); AlphaFilteredYoVariable filteredDesired = filteredDesiredJointAngles.get(simulatedJoint); double qDesired = -MathTools.clamp(tau / stiffness.getDoubleValue(), maxDeflection.getDoubleValue()); filteredDesired.update(qDesired); simulatedJoint.setQ(filteredDesired.getDoubleValue()); } }
public void update() { read(); accel2quaternions(accel, pYawMagnet.getDoubleValue()); xsens.getQuaternion(xsensQuat); xsensMatrix.set(xsensQuat); YawPitchRollConversion.convertQuaternionToYawPitchRoll(xsensQuat, yawPitchRoll); xsens_yaw.update(yawPitchRoll[0]); xsens_pitch.update(yawPitchRoll[1]); xsens_roll.update(yawPitchRoll[2]); q_calc_torso_x.set(torso_roll.getDoubleValue() - xsens_roll.getDoubleValue()); q_calc_torso_y.set(torso_pitch.getDoubleValue() - xsens_pitch.getDoubleValue()); q_calc_torso_z.set(0.0); }
private void computePrivilegedJointVelocitiesForPriviligedJointAngles(DenseMatrix64F privilegedJointVelocitiesToPack, double weight) { for (int i = 0; i < numberOfDoF; i++) { yoPrivilegedJointPositionsFiltered[i].update(); privilegedJointVelocitiesToPack.set(i, 0, -2.0 * weight * (localJoints[i].getQ() - yoPrivilegedJointPositionsFiltered[i].getDoubleValue()) / jointSquaredRangeOfMotions.get(i, 0)); } }
@Override public void notifyOfVariableChange(YoVariable<?> v) { alphaFilteredHeadYawPercentage.update(headYawPercentage.getDoubleValue()); NeckJointName headYaw = NeckJointName.DISTAL_NECK_YAW; double neckYawJointRange = sliderBoardControlledNeckJointsWithLimits.get(headYaw).getRight() - sliderBoardControlledNeckJointsWithLimits.get(headYaw).getLeft(); YoDouble desiredAngle = (YoDouble) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(headYaw) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredHeadYawPercentage.getDoubleValue() * neckYawJointRange + sliderBoardControlledNeckJointsWithLimits.get(headYaw).getLeft()); } });
@Override public void notifyOfVariableChange(YoVariable<?> v) { alphaFilteredUpperHeadPitchPercentage.update(upperHeadPitchPercentage.getDoubleValue()); NeckJointName upperHeadPitch = NeckJointName.DISTAL_NECK_PITCH; double jointRange = sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getRight() - sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getLeft(); YoDouble desiredAngle = (YoDouble) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(upperHeadPitch) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredUpperHeadPitchPercentage.getDoubleValue() * jointRange + sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getLeft()); } });
@Override public void notifyOfVariableChange(YoVariable<?> v) { alphaFilteredLowerHeadPitchYawPercentage.update(lowerHeadPitchPercentage.getDoubleValue()); NeckJointName lowerHeadPitch = NeckJointName.PROXIMAL_NECK_PITCH; double jointRange = sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getRight() - sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getLeft(); YoDouble desiredAngle = (YoDouble) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(lowerHeadPitch) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredLowerHeadPitchYawPercentage.getDoubleValue() * jointRange + sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getLeft()); } });
@Override public void variableChanged(YoVariable<?> v) { alphaFilteredUpperHeadPitchPercentage.update(upperHeadPitchPercentage.getDoubleValue()); NeckJointName upperHeadPitch = NeckJointName.DISTAL_NECK_PITCH; double jointRange = sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getRight() - sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getLeft(); DoubleYoVariable desiredAngle = (DoubleYoVariable) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(upperHeadPitch) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredUpperHeadPitchPercentage.getDoubleValue() * jointRange + sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getLeft()); } });
@Override public void variableChanged(YoVariable<?> v) { alphaFilteredLowerHeadPitchYawPercentage.update(lowerHeadPitchPercentage.getDoubleValue()); NeckJointName lowerHeadPitch = NeckJointName.PROXIMAL_NECK_PITCH; double jointRange = sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getRight() - sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getLeft(); DoubleYoVariable desiredAngle = (DoubleYoVariable) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(lowerHeadPitch) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredLowerHeadPitchYawPercentage.getDoubleValue() * jointRange + sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getLeft()); } });
@Override public void variableChanged(YoVariable<?> v) { alphaFilteredUpperHeadPitchPercentage.update(upperHeadPitchPercentage.getDoubleValue()); NeckJointName upperHeadPitch = NeckJointName.DISTAL_NECK_PITCH; double jointRange = sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getRight() - sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getLeft(); DoubleYoVariable desiredAngle = (DoubleYoVariable) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(upperHeadPitch) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredUpperHeadPitchPercentage.getDoubleValue() * jointRange + sliderBoardControlledNeckJointsWithLimits.get(upperHeadPitch).getLeft()); } });
@Override public void variableChanged(YoVariable<?> v) { alphaFilteredHeadYawPercentage.update(headYawPercentage.getDoubleValue()); NeckJointName headYaw = NeckJointName.DISTAL_NECK_YAW; double neckYawJointRange = sliderBoardControlledNeckJointsWithLimits.get(headYaw).getRight() - sliderBoardControlledNeckJointsWithLimits.get(headYaw).getLeft(); DoubleYoVariable desiredAngle = (DoubleYoVariable) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(headYaw) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredHeadYawPercentage.getDoubleValue() * neckYawJointRange + sliderBoardControlledNeckJointsWithLimits.get(headYaw).getLeft()); } });
@Override public void variableChanged(YoVariable<?> v) { alphaFilteredLowerHeadPitchYawPercentage.update(lowerHeadPitchPercentage.getDoubleValue()); NeckJointName lowerHeadPitch = NeckJointName.PROXIMAL_NECK_PITCH; double jointRange = sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getRight() - sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getLeft(); DoubleYoVariable desiredAngle = (DoubleYoVariable) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(lowerHeadPitch) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredLowerHeadPitchYawPercentage.getDoubleValue() * jointRange + sliderBoardControlledNeckJointsWithLimits.get(lowerHeadPitch).getLeft()); } });
@Override public void variableChanged(YoVariable<?> v) { alphaFilteredHeadYawPercentage.update(headYawPercentage.getDoubleValue()); NeckJointName headYaw = NeckJointName.DISTAL_NECK_YAW; double neckYawJointRange = sliderBoardControlledNeckJointsWithLimits.get(headYaw).getRight() - sliderBoardControlledNeckJointsWithLimits.get(headYaw).getLeft(); DoubleYoVariable desiredAngle = (DoubleYoVariable) registry.getVariable(drcRobotModel.getJointMap().getNeckJointName(headYaw) + "_unconstrained" + CommonNames.q_d); desiredAngle.set(alphaFilteredHeadYawPercentage.getDoubleValue() * neckYawJointRange + sliderBoardControlledNeckJointsWithLimits.get(headYaw).getLeft()); } });
public void update(double currentPosition) { if (!hasBeenCalled.getBooleanValue()) { hasBeenCalled.set(true); set(currentPosition); } else { double alpha = alphaVariable.getValue(); set(alpha * getDoubleValue() + (1.0 - alpha) * currentPosition); } }
public double computeFootLoadPercentage() { readSensorData(footWrench); yoFootForceInFoot.getFrameTupleIncludingFrame(footForce); footForce.clipToMinMax(0.0, Double.MAX_VALUE); footForceMagnitude.set(footForce.length()); footLoadPercentage.update(footForce.getZ() / robotTotalWeight); return footLoadPercentage.getDoubleValue(); }
/** * clips max translational velocity */ private void updateTranslationalMaxVelocityClip() { interpolatedTranslationCorrectionFrame.getTransformToDesiredFrame(errorBetweenCurrentPositionAndCorrected, worldFrame); errorBetweenCurrentPositionAndCorrected.getTranslation(distanceToTravelVector); distanceToTravel.set(distanceToTravelVector.length()); maxTranslationAlpha.set((estimatorDT * maxTranslationVelocityClip.getDoubleValue() / distanceToTravel.getDoubleValue()) + previousTranslationClippedAlphaValue.getDoubleValue()); translationClippedAlphaValue.set(MathTools.clipToMinMax(interpolationTranslationAlphaFilter.getDoubleValue(), 0.0, maxTranslationAlpha.getDoubleValue())); previousTranslationClippedAlphaValue.set(translationClippedAlphaValue.getDoubleValue()); }
/** * clips max rotational velocity */ private void updateRotationalMaxVelocityClip() { interpolatedRotationCorrectionFrame.getTransformToDesiredFrame(errorBetweenCurrentPositionAndCorrected, worldFrame); errorBetweenCurrentPositionAndCorrected.getRotation(angleToTravelAxis4d); angleToTravel.set(angleToTravelAxis4d.getAngle()); maxRotationAlpha.set((estimatorDT * maxRotationVelocityClip.getDoubleValue() / angleToTravel.getDoubleValue()) + previousRotationClippedAlphaValue.getDoubleValue()); rotationClippedAlphaValue.set(MathTools.clipToMinMax(interpolationRotationAlphaFilter.getDoubleValue(), 0.0, maxRotationAlpha.getDoubleValue())); previousRotationClippedAlphaValue.set(rotationClippedAlphaValue.getDoubleValue()); }