public void setAndUpdate(RigidBodyTransform transform) { transform.get(rotation, translation); setAndUpdate(rotation, translation); }
public void setAndUpdate(RigidBodyTransform transform) { transform.get(rotation, translation); setAndUpdate(rotation, translation); }
/** * 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 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); }
/** * 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()); }
/** * 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); }
@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(); } }