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); }
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); }
/** * 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); }
public void setAndUpdate(RigidBodyTransform transform) { transform.get(rotation, translation); setAndUpdate(rotation, translation); }
/** * sets initials for correction and calculates error in past */ private void addNewExternalPose(TimeStampedTransform3D newPelvisPoseWithTime) { previousTranslationClippedAlphaValue.set(0.0); interpolationTranslationAlphaFilter.set(0.0); distanceTraveled.set(0.0); previousRotationClippedAlphaValue.set(0.0); interpolationRotationAlphaFilter.set(0.0); angleTraveled.set(0.0); calculateAndStoreErrorInPast(newPelvisPoseWithTime); interpolationRotationStartFrame.setAndUpdate(interpolatedRotationCorrectionFrame.getTransformToParent()); interpolationTranslationStartFrame.setAndUpdate(interpolatedTranslationCorrectionFrame.getTransformToParent()); }
/** * 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()); }
private void sendCorrectionUpdatePacket() { totalRotationErrorFrame.get(totalRotationError); totalTranslationErrorFrame.get(totalTranslationError); totalError.set(totalRotationError, totalTranslationError); errorBetweenCurrentPositionAndCorrected.getTranslation(translationalResidualError); double absoluteResidualError = translationalResidualError.length(); totalError.getTranslation(translationalTotalError); double absoluteTotalError = translationalTotalError.length(); PelvisPoseErrorPacket pelvisPoseErrorPacket = new PelvisPoseErrorPacket((float) absoluteTotalError, (float) absoluteResidualError, false); pelvisPoseCorrectionCommunicator.sendPelvisPoseErrorPacket(pelvisPoseErrorPacket); }
pelvisStateAtLocalizationTimeTranslationFrame = new YoReferencePose("pelvisStateAtLocalizationTimeTranslationFrame", worldFrame, registry); pelvisStateAtLocalizationTimeRotationFrame = new YoReferencePose("pelvisStateAtLocalizationTimeRotationFrame", worldFrame, registry); newLocalizationTranslationFrame = new YoReferencePose("newLocalizationTranslationFrame", worldFrame, registry); newLocalizationRotationFrame = new YoReferencePose("newLocalizationRotationFrame", worldFrame, registry); interpolationTranslationStartFrame = new YoReferencePose("interpolationTranslationStartFrame", worldFrame, registry); interpolationRotationStartFrame = new YoReferencePose("interpolationRotationStartFrame", worldFrame, registry); totalTranslationErrorFrame = new YoReferencePose("totalTranslationErrorFrame", worldFrame, registry); totalRotationErrorFrame = new YoReferencePose("totalRotationErrorFrame", worldFrame, registry); interpolatedTranslationCorrectionFrame = new YoReferencePose("interpolatedTranslationCorrectionFrame", worldFrame, registry); interpolatedRotationCorrectionFrame = new YoReferencePose("interpolatedRotationCorrectionFrame", worldFrame, registry); translationCorrection = new YoReferencePose("translationCorrection", worldFrame, registry); nonCorrectedPelvis = new YoReferencePose("nonCorrectedPelvis", translationCorrection, registry); orientationCorrection = new YoReferencePose("orientationCorrection", nonCorrectedPelvis, registry); correctedPelvis = new YoReferencePose("correctedPelvis", worldFrame, registry);
/** * 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); }
public void setAndUpdate(RigidBodyTransform transform) { transform.get(rotation, translation); setAndUpdate(rotation, translation); }
/** * 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()); }
@Override public void doControl(long timestamp) { if (pelvisPoseCorrectionCommunicator != null) { pelvisReferenceFrame.update(); checkForManualTrigger(); checkForNewPacket(); interpolationTranslationAlphaFilter.update(confidenceFactor.getDoubleValue()); interpolationRotationAlphaFilter.update(confidenceFactor.getDoubleValue()); pelvisReferenceFrame.getTransformToParent(pelvisPose); addPelvisePoseToPelvisBuffer(pelvisPose, timestamp); nonCorrectedPelvis.setAndUpdate(pelvisPose); correctPelvisPose(pelvisPose); correctedPelvis.setAndUpdate(pelvisPose); rootJoint.setPositionAndRotation(pelvisPose); pelvisReferenceFrame.update(); checkForNeedToSendCorrectionUpdate(); } }