本文整理了Java中us.ihmc.robotics.math.YoReferencePose类的一些代码示例，展示了YoReferencePose类的具体用法。这些代码示例主要来源于Github/Stackoverflow/Maven等平台，是从一些精选项目中提取出来的代码，具有较强的参考意义，能在一定程度帮忙到你。YoReferencePose类的具体详情如下：
包路径：us.ihmc.robotics.math.YoReferencePose
类名称：YoReferencePose

YoReferencePose介绍

暂无

代码示例

代码示例来源：origin: us.ihmc/ihmc-robotics-toolkit

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);
}

代码示例来源：origin: us.ihmc/ihmc-robotics-toolkit

public void setAndUpdate(Vector3D newTranslation)
{
 set(newTranslation);
 update();
}

代码示例来源：origin: us.ihmc/IHMCRoboticsToolkit

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);
}

代码示例来源：origin: us.ihmc/IHMCStateEstimation

/**
* 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);
}

代码示例来源：origin: us.ihmc/ihmc-robotics-toolkit

public void setAndUpdate(RigidBodyTransform transform)
{
 transform.get(rotation, translation);
 setAndUpdate(rotation, translation);
}

代码示例来源：origin: us.ihmc/IHMCStateEstimation

/**
* 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());
}

代码示例来源：origin: us.ihmc/IHMCStateEstimation

/**
* 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());
}

代码示例来源：origin: us.ihmc/IHMCStateEstimation

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);
}

代码示例来源：origin: us.ihmc/IHMCStateEstimation

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);

代码示例来源：origin: us.ihmc/IHMCStateEstimation

/**
* 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);
}

代码示例来源：origin: us.ihmc/IHMCRoboticsToolkit

public void setAndUpdate(RigidBodyTransform transform)
{
 transform.get(rotation, translation);
 setAndUpdate(rotation, translation);
}

代码示例来源：origin: us.ihmc/IHMCStateEstimation

/**
* 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());
}

代码示例来源：origin: us.ihmc/IHMCRoboticsToolkit

public void setAndUpdate(Vector3d newTranslation)
{
 set(newTranslation);
 update();
}

代码示例来源：origin: us.ihmc/IHMCStateEstimation

@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();
 }
}

代码示例来源：origin: us.ihmc/IHMCRoboticsToolkit

public void setAndUpdate(Quat4d newRotation)
{
 set(newRotation);
 update();
}

代码示例来源：origin: us.ihmc/IHMCRoboticsToolkit

public void setAndUpdate(Quat4d newRotation, Vector3d newTranslation)
{
 set(newRotation);
 set(newTranslation);
 update();
}

代码示例来源：origin: us.ihmc/ihmc-robotics-toolkit

public void setAndUpdate(Quaternion newRotation)
{
 set(newRotation);
 update();
}

代码示例来源：origin: us.ihmc/ihmc-robotics-toolkit

public void setAndUpdate(Quaternion newRotation, Vector3D newTranslation)
{
 set(newRotation);
 set(newTranslation);
 update();
}