us.ihmc.yoVariables.variable.YoFrameVector3D类的使用及代码示例

x33g5p2x  于2022-02-05 转载在 其他  
字(15.7k)|赞(0)|评价(0)|浏览(138)

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

YoFrameVector3D介绍

[英]FixedFrameVector3DBasics implementation which components x, y, zare baked with YoDoubles.
[中]FixedFrameVector3DBasics实现，其中的x、y、zare组件用Yodouble烘焙。

代码示例

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

public YoVelocityProvider(String name, ReferenceFrame referenceFrame, YoVariableRegistry registry)
{
 this.frameVector = new YoFrameVector3D(name, referenceFrame, registry);
}

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

public void setAngularVelocity(FrameVector3D angularVelocity)
{
 this.angularVelocity.set(angularVelocity);
}
public void set(SO3TrajectoryPointInterface<?> so3TrajectoryPoint)

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

private void computeIntegralTerm()
{
 if (gains.getMaximumIntegralError() < 1e-5)
 {
   integralTerm.setToZero(bodyFrame);
   return;
 }
 double errorIntegratedX = positionError.getX() * dt;
 double errorIntegratedY = positionError.getY() * dt;
 double errorIntegratedZ = positionError.getZ() * dt;
 positionErrorCumulated.add(errorIntegratedX, errorIntegratedY, errorIntegratedZ);
 double errorMagnitude = positionErrorCumulated.length();
 if (errorMagnitude > gains.getMaximumIntegralError())
 {
   positionErrorCumulated.scale(gains.getMaximumIntegralError() / errorMagnitude);
 }
 integralTerm.set(positionErrorCumulated);
 gains.getIntegralGainMatrix(tempGainMatrix);
 tempGainMatrix.transform(integralTerm);
}

代码示例来源：origin: us.ihmc/simulation-construction-set-tools

public void computeCurrentDisturbanceForce()
{
 currentDisturbanceForce.setX(random.nextDouble());
 currentDisturbanceForce.setY(random.nextDouble());
 currentDisturbanceForce.setZ(random.nextDouble());
 currentDisturbanceForce.normalize();
 currentDisturbanceForce.scale(maximalDisturbanceMagnitude.getDoubleValue());
}

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

public void initialize(FramePoint3D initialPosition, FrameVector3D initialVelocity, FramePoint3D finalPosition)
{
 initialPosition.changeFrame(referenceFrame);
 initialVelocity.changeFrame(referenceFrame);
 finalPosition.changeFrame(referenceFrame);
 
 c0.set(initialPosition);
 c1.set(initialVelocity);
 c2.set(finalPosition);
 c2.sub(initialPosition);
 c2.sub(initialVelocity);
}

代码示例来源：origin: us.ihmc/ihmc-graphics-description

@Override
public YoDouble[] getVariables()
{
 return new YoDouble[] {base.getYoX(), base.getYoY(), base.getYoZ(), vector.getYoX(), vector.getYoY(), vector.getYoZ()};
}

代码示例来源：origin: us.ihmc/ihmc-common-walking-control-modules-test

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testComputeDesiredCapturePointVelocity()
{
 YoFramePoint3D initialCapturePointPosition = new YoFramePoint3D("", ReferenceFrame.getWorldFrame(), registry);
 YoFramePoint3D computedCapturePoint1 = new YoFramePoint3D("1", ReferenceFrame.getWorldFrame(), registry);
 YoFramePoint3D computedCapturePoint2 = new YoFramePoint3D("2", ReferenceFrame.getWorldFrame(), registry);
 YoFramePoint3D initialCenterOfPressure = new YoFramePoint3D("3", ReferenceFrame.getWorldFrame(), registry);
 YoFrameVector3D differentiatedCapturePointPosition = new YoFrameVector3D("4", ReferenceFrame.getWorldFrame(), registry);
 YoFrameVector3D computedCapturePointVelocity = new YoFrameVector3D("5", ReferenceFrame.getWorldFrame(), registry);
 for (int i = 0; i < nTests; i++)
 {
   initialCapturePointPosition.set(random.nextDouble(), random.nextDouble(), 0);
   initialCenterOfPressure.set(initialCapturePointPosition.getX() + 0.02, initialCapturePointPosition.getY() + 0.01, 0);
   double deltaT = 0.001;
   double time = random.nextDouble() * 0.1 + 0.05;
   double omega0 = 0.5;
   CapturePointTools.computeDesiredCapturePointPosition(omega0, time, initialCapturePointPosition, initialCenterOfPressure, computedCapturePoint1);
   CapturePointTools.computeDesiredCapturePointPosition(omega0, time + deltaT, initialCapturePointPosition, initialCenterOfPressure,
      computedCapturePoint2);
   differentiatedCapturePointPosition.set(computedCapturePoint2);
   differentiatedCapturePointPosition.sub(computedCapturePoint1);
   differentiatedCapturePointPosition.scale(1 / deltaT);
   CapturePointTools.computeDesiredCapturePointVelocity(omega0, time + deltaT, initialCapturePointPosition, initialCenterOfPressure,
      computedCapturePointVelocity);
   EuclidCoreTestTools.assertTuple3DEquals("", computedCapturePointVelocity, differentiatedCapturePointPosition, 1e-3);
 }
}

代码示例来源：origin: us.ihmc/ihmc-graphics-description

public YoGraphicVector(String name, YoFramePoint3D startPoint, YoFrameVector3D frameVector, double scale, AppearanceDefinition appearance,
           boolean drawArrowhead)
{
 this(name, startPoint.getYoX(), startPoint.getYoY(), startPoint.getYoZ(), frameVector.getYoX(), frameVector.getYoY(), frameVector.getYoZ(), scale,
    appearance, drawArrowhead);
 if (!startPoint.getReferenceFrame().isWorldFrame() || !frameVector.getReferenceFrame().isWorldFrame())
 {
   System.err.println("Warning: Should be in a World Frame to create a YoGraphicVector. startPoint = " + startPoint + ", frameVector = " + frameVector);
 }
}

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

YoFrameQuaternion actualOrientation = new YoFrameQuaternion("q_act_", sensorFrame, registry);
YoFrameVector3D inputAngularVelocity = new YoFrameVector3D("angularVelocity", sensorFrame, registry);
YoFrameVector3D inputLinearAcceleration = new YoFrameVector3D("linearAcceleration", sensorFrame, registry);
YoFrameVector3D inputMagneticVector = new YoFrameVector3D("magneticDirection", sensorFrame, registry);
YoIMUMahonyFilter mahonyFilter = new YoIMUMahonyFilter(imuName, namePrefix, nameSuffix, updateDT, sensorFrame, registry);
mahonyFilter.setInputs(inputAngularVelocity, inputLinearAcceleration, inputMagneticVector);
Vector3D zUp = new Vector3D(0.0, 0.0, 1.0);
actualOrientation.inverseTransform(zUp);
inputLinearAcceleration.set(zUp);
inputLinearAcceleration.scale(20.0);
inputMagneticVector.set(xForward);
inputMagneticVector.scale(0.5);

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

private void applyForce()
{
 double length = pushDirection.length();
 if (length > 1e-5)
 {
   pushForce.set(pushDirection);
   pushForce.normalize();
   pushForce.scale(pushForceMagnitude.getDoubleValue());
   if (pushCondition == null)
   {
    pushTimeSwitch.set(yoTime.getDoubleValue());
   }
 }
 else
 {
   pushForce.setToZero();
   pushTimeSwitch.set(Double.NEGATIVE_INFINITY);
 }
 pushNumber.increment();
}

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

public void initialize(FramePoint3D initialPosition, FramePoint3D intermediatePosition, FramePoint3D finalPosition, double intermediateParameter)
{
 initialPosition.changeFrame(referenceFrame);
 intermediatePosition.changeFrame(referenceFrame);
 finalPosition.changeFrame(referenceFrame);
 final double q = intermediateParameter;
 MathTools.checkIntervalContains(q, 0.0, 1.0);
 c0.set(initialPosition);
 c2.set(intermediatePosition);
 c2.sub(initialPosition);
 tempInitialize.set(finalPosition);
 tempInitialize.sub(initialPosition);
 tempInitialize.scale(q);
 c2.sub(tempInitialize);
 c2.scale(1.0 / (MathTools.square(q) - q));
 c1.set(finalPosition);
 c1.sub(initialPosition);
 c1.sub(c2);
}

代码示例来源：origin: us.ihmc/ihmc-footstep-planning

public static void addGoalViz(FramePose3D goalPose, YoVariableRegistry registry, YoGraphicsListRegistry graphicsListRegistry)
  {
   YoFramePoint3D yoGoal = new YoFramePoint3D("GoalPosition", worldFrame, registry);
   yoGoal.set(goalPose.getPosition());
   graphicsListRegistry.registerYoGraphic("viz", new YoGraphicPosition("GoalViz", yoGoal, 0.05, YoAppearance.Yellow()));
   YoFramePoint3D yoStart = new YoFramePoint3D("StartPosition", worldFrame, registry);
   graphicsListRegistry.registerYoGraphic("viz", new YoGraphicPosition("StartViz", yoStart, 0.05, YoAppearance.Blue()));
   PoseReferenceFrame goalFrame = new PoseReferenceFrame("GoalFrame", goalPose);
   FrameVector3D goalOrientation = new FrameVector3D(goalFrame, 0.5, 0.0, 0.0);
   goalOrientation.changeFrame(worldFrame);
   YoFrameVector3D yoGoalOrientation = new YoFrameVector3D("GoalVector", worldFrame, registry);
   yoGoalOrientation.set(goalOrientation);
//      graphicsListRegistry.registerYoGraphic("vizOrientation", new YoGraphicVector("GoalOrientationViz", yoGoal, yoGoalOrientation, 1.0, YoAppearance.White()));
  }

代码示例来源：origin: us.ihmc/ihmc-avatar-interfaces-test

private void generateRandomSlipParamters()
{
 double randomSlipTranslateX = pseudoRandomRealNumberWithinRange(minTranslationToSlipNextStep.getX(), maxTranslationToSlipNextStep.getX());
 double randomSlipTranslateY = pseudoRandomRealNumberWithinRange(minTranslationToSlipNextStep.getY(), maxTranslationToSlipNextStep.getY());
 double randomSlipTranslateZ = pseudoRandomRealNumberWithinRange(minTranslationToSlipNextStep.getZ(), maxTranslationToSlipNextStep.getZ());
 nextTranslationToSlip.set(randomSlipTranslateX, randomSlipTranslateY, randomSlipTranslateZ);
 nextRotationToSlip.setYawPitchRoll(pseudoRandomRealNumberWithinRange(minRotationToSlipNextStep.getYawPitchRoll(),
    maxRotationToSlipNextStep.getYawPitchRoll()));
 double randomSlipAfterTimeDelta = pseudoRandomPositiveNumberWithinRange(minSlipAfterTimeDelta.getDoubleValue(), maxSlipAfterTimeDelta.getDoubleValue());
 double randomPercentToSlipPerTick = pseudoRandomPositiveNumberWithinRange(minSlipPercentSlipPerTick.getDoubleValue(),
    maxSlipPercentSlipPerTick.getDoubleValue());
 nextSlipAfterTimeDelta.set(randomSlipAfterTimeDelta);
 nextSlipPercentSlipPerTick.set(randomPercentToSlipPerTick);
}

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

groundContactPointsSlipper.setDoSlip(true);
translationPhase.setX(translationPhase.getX() + 2.0  * Math.PI * translationFreqHz[0] * deltaT);
translationPhase.setY(translationPhase.getY() + 2.0  * Math.PI * translationFreqHz[1] * deltaT);
translationPhase.setZ(translationPhase.getZ() + 2.0  * Math.PI * translationFreqHz[2] * deltaT);
rotationPhaseEuler.setX(rotationPhaseEuler.getX() + 2.0  * Math.PI * rotationFreqHzYawPitchRoll[2] * deltaT);
rotationPhaseEuler.setY(rotationPhaseEuler.getY() + 2.0  * Math.PI * rotationFreqHzYawPitchRoll[1] * deltaT);
rotationPhaseEuler.setZ(rotationPhaseEuler.getZ() + 2.0  * Math.PI * rotationFreqHzYawPitchRoll[0] * deltaT);
nextTranslationToSlip.setX(nextTranslationToSlip.getX() * (2.0 * Math.PI * translationFreqHz[0] * Math.sin(translationPhase.getX()) * deltaT));
nextTranslationToSlip.setY(nextTranslationToSlip.getY() * (2.0 * Math.PI * translationFreqHz[1] * Math.sin(translationPhase.getY()) * deltaT));
nextTranslationToSlip.setZ(nextTranslationToSlip.getZ() * (2.0 * Math.PI * translationFreqHz[2] * Math.sin(translationPhase.getZ()) * deltaT));
nextRotationToSlipEulerAngles.setX(nextRotationToSlipEulerAngles.getX() * (2.0 * Math.PI * rotationFreqHzYawPitchRoll[2] * Math.sin(rotationPhaseEuler.getX()) * deltaT));
nextRotationToSlipEulerAngles.setY(nextRotationToSlipEulerAngles.getY() * (2.0 * Math.PI * rotationFreqHzYawPitchRoll[1] * Math.sin(rotationPhaseEuler.getY()) * deltaT));
nextRotationToSlipEulerAngles.setZ(nextRotationToSlipEulerAngles.getZ() * (2.0 * Math.PI * rotationFreqHzYawPitchRoll[0] * Math.sin(rotationPhaseEuler.getZ()) * deltaT));

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

@Override
public void initialize()
{
 currentTime.set(0.0);
 initialAngularVelocityMagnitude.set(initialAngularVelocity.length());
 finalAngularVelocityMagnitude.set(finalAngularVelocity.length());
 maxAngularVelocityMagnitudeAtLimits.set(PI / trajectoryTime.getDoubleValue());
 initialDriftSaturated.set(initialAngularVelocityMagnitude.getDoubleValue() > maxAngularVelocityMagnitudeAtLimits.getDoubleValue());
 finalDriftSaturated.set(finalAngularVelocityMagnitude.getDoubleValue() > maxAngularVelocityMagnitudeAtLimits.getDoubleValue());
 parameterPolynomial.setQuintic(0, trajectoryTime.getDoubleValue(), 0.0, 0.0, 0.0, 1.0, 0.0, 0.0);
 if (initialOrientation.dot(finalOrientation) < 0.0)
   finalOrientation.negate();
 currentOrientation.set(initialOrientation);
 currentAngularVelocity.set(initialAngularVelocity);
 currentAngularAcceleration.setToZero();
}

代码示例来源：origin: us.ihmc/ihmc-common-walking-control-modules-test

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testComputeDesiredCapturePointAcceleration()
{
 YoFramePoint3D initialCapturePointPosition = new YoFramePoint3D("", ReferenceFrame.getWorldFrame(), registry);
 YoFramePoint3D initialCenterOfPressure = new YoFramePoint3D("3", ReferenceFrame.getWorldFrame(), registry);
 YoFrameVector3D computedCapturePointVelocity = new YoFrameVector3D("5", ReferenceFrame.getWorldFrame(), registry);
 YoFrameVector3D desiredCapturePointAcceleration = new YoFrameVector3D("6", ReferenceFrame.getWorldFrame(), registry);
 for (int i = 0; i < nTests; i++)
 {
   initialCapturePointPosition.set(random.nextDouble(), random.nextDouble(), 0);
   initialCenterOfPressure.set(initialCapturePointPosition.getX() + 0.02, initialCapturePointPosition.getY() + 0.01, 0);
   double time = random.nextDouble() * 0.1 + 0.05;
   double omega0 = 0.5;
   CapturePointTools.computeDesiredCapturePointVelocity(omega0, time, initialCapturePointPosition, initialCenterOfPressure, computedCapturePointVelocity);
   CapturePointTools.computeDesiredCapturePointAcceleration(omega0, time, initialCapturePointPosition, initialCenterOfPressure,
      desiredCapturePointAcceleration);
   computedCapturePointVelocity.scale(omega0);
   EuclidCoreTestTools.assertTuple3DEquals("", computedCapturePointVelocity, desiredCapturePointAcceleration, 1e-10);
   computedCapturePointVelocity.scale(1 / omega0);
   CapturePointTools.computeDesiredCapturePointAcceleration(omega0, computedCapturePointVelocity, desiredCapturePointAcceleration);
   computedCapturePointVelocity.scale(omega0);
   EuclidCoreTestTools.assertTuple3DEquals("", computedCapturePointVelocity, desiredCapturePointAcceleration, 1e-10);
 }
}

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

private void computeProportionalTerm(FramePoint3D desiredPosition)
{
 desiredPosition.changeFrame(bodyFrame);
 positionError.set(desiredPosition);
 // Limit the maximum position error considered for control action
 double maximumError = gains.getMaximumProportionalError();
 double errorMagnitude = positionError.length();
 proportionalTerm.set(positionError);
 if (errorMagnitude > maximumError)
 {
   proportionalTerm.scale(maximumError / errorMagnitude);
 }
 gains.getProportionalGainMatrix(tempGainMatrix);
 tempGainMatrix.transform(proportionalTerm);
}

代码示例来源：origin: us.ihmc/simulation-construction-set-test

@Test// timeout=300000
public void testGetYoVelocity()
{
  YoFrameVector3D yoVelocity = kinematicPoint.getYoVelocity();
  String frameName = yoVelocity.getReferenceFrame().getName();
  
 assertEquals("( 0.000,  0.000,  0.000 )-" + frameName, yoVelocity.toString());
  yoVelocity.set(new Vector3D(5.0, 5.1, 5.2));
  assertEquals("( 5.000,  5.100,  5.200 )-" + frameName, yoVelocity.toString());
}

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

public YoIMUMahonyFilter(String imuName, String nameSuffix, double updateDT, ReferenceFrame sensorFrame, YoFrameQuaternion estimatedOrientation,
            YoFrameVector3D estimatedAngularVelocity, YoVariableRegistry parentRegistry)
{
 this.updateDT = updateDT;
 this.sensorFrame = sensorFrame;
 YoVariableRegistry registry = new YoVariableRegistry(imuName + "MahonyFilter");
 parentRegistry.addChild(registry);
 estimatedOrientation.checkReferenceFrameMatch(sensorFrame);
 if (estimatedAngularVelocity != null)
   estimatedAngularVelocity.checkReferenceFrameMatch(sensorFrame);
 this.estimatedOrientation = estimatedOrientation;
 this.estimatedAngularVelocity = estimatedAngularVelocity;
 yoErrorTerm = new YoFrameVector3D("ErrorTerm", nameSuffix, sensorFrame, registry);
 yoIntegralTerm = new YoFrameVector3D("IntegralTerm", nameSuffix, sensorFrame, registry);
 proportionalGain = new YoDouble("ProportionalGain" + nameSuffix, registry);
 integralGain = new YoDouble("IntegralGain" + nameSuffix, registry);
 hasBeenInitialized = new YoBoolean("HasBeenInitialized" + nameSuffix, registry);
}

代码示例来源：origin: us.ihmc/simulation-construction-set-tools

@Override
public double[] getMessageValues()
{
 return new double[]
 {
   force.getX(), force.getY(), force.getZ(), position.getX(), position.getY(), position.getZ(),
   groundContactPoint.getYoFootSwitch().getDoubleValue()
 };
}

内容来源于网络，如有侵权，请联系作者删除！

YoFrameVector3D

相关文章

    查看更多

    热门标签

    更多
    JavaquerypythonNode开发语言requestUtil数据库Table后端算法LoggerMessageElementParser

    最新文章

    更多
    • 蜀ICP备13028337号-1 大数据知识库 https://www.saoniuhuo.com © All rights reserved
    • 本站内容来源互联网,如果侵犯您的权益请联系我们删除, 联系方式：448109455@qq.com