public void interpolate(YoReferencePose start, YoReferencePose goal, double alpha) { start.getTransformToDesiredFrame(interpolationStartingPosition, parentFrame); goal.getTransformToDesiredFrame(interpolationGoalPosition, parentFrame); transformInterpolationCalculator.computeInterpolation(interpolationStartingPosition, interpolationGoalPosition, output, alpha); setAndUpdate(output); }
public void interpolate(YoReferencePose start, YoReferencePose goal, double alpha) { start.getTransformToDesiredFrame(interpolationStartingPosition, getParent()); goal.getTransformToDesiredFrame(interpolationGoalPosition, getParent()); transformInterpolationCalculator.computeInterpolation(interpolationStartingPosition, interpolationGoalPosition, output, alpha); setAndUpdate(output); }
/** * Calculates the difference between the external at t with the state estimated pelvis pose at t and stores it * @param localizationPose - the corrected pelvis pose */ public void calculateAndStoreErrorInPast(TimeStampedTransform3D timestampedlocalizationPose) { long timeStamp = timestampedlocalizationPose.getTimeStamp(); RigidBodyTransform localizationPose = timestampedlocalizationPose.getTransform3D(); localizationPose.getTranslation(localizationTranslationInPast); newLocalizationTranslationFrame.setAndUpdate(localizationTranslationInPast); localizationPose.getRotation(localizationRotationInPast); newLocalizationRotationFrame.setAndUpdate(localizationRotationInPast); stateEstimatorPelvisPoseBuffer.findTransform(timeStamp, seTimeStampedPose); RigidBodyTransform sePose = seTimeStampedPose.getTransform3D(); sePose.getTranslation(seTranslationInPast); pelvisStateAtLocalizationTimeTranslationFrame.setAndUpdate(seTranslationInPast); sePose.getRotation(seRotationInPast); pelvisStateAtLocalizationTimeRotationFrame.setAndUpdate(seRotationInPast); newLocalizationTranslationFrame.getTransformToDesiredFrame(translationErrorInPastTransform, pelvisStateAtLocalizationTimeTranslationFrame); newLocalizationRotationFrame.getTransformToDesiredFrame(rotationErrorInPastTransform, pelvisStateAtLocalizationTimeRotationFrame); totalTranslationErrorFrame.setAndUpdate(translationErrorInPastTransform); totalRotationErrorFrame.setAndUpdate(rotationErrorInPastTransform); }
/** * 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()); }
/** * 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()); }
/** * Updates max velocity clipping, interpolates from where we were * at the last correction tick to the goal and updates the pelvis * @param pelvisPose - non corrected pelvis position */ private void correctPelvisPose(RigidBodyTransform pelvisPose) { updateTranslationalMaxVelocityClip(); updateRotationalMaxVelocityClip(); interpolatedRotationCorrectionFrame.interpolate(interpolationRotationStartFrame, totalRotationErrorFrame, rotationClippedAlphaValue.getDoubleValue()); interpolatedTranslationCorrectionFrame.interpolate(interpolationTranslationStartFrame, totalTranslationErrorFrame, translationClippedAlphaValue.getDoubleValue()); if (USE_ROTATION_CORRECTION) interpolatedRotationCorrectionFrame.getTransformToParent(interpolatedRotationError); else interpolatedRotationError.setIdentity(); orientationCorrection.setAndUpdate(interpolatedRotationError); interpolatedTranslationCorrectionFrame.getTransformToParent(interpolatedTranslationError); translationCorrection.setAndUpdate(interpolatedTranslationError); orientationCorrection.getTransformToDesiredFrame(pelvisPose, worldFrame); }