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

YoFramePointInMultipleFrames介绍

暂无

代码示例

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

public ConstantPoseTrajectoryGenerator(String namePrefix, boolean allowMultipleFrames, ReferenceFrame referenceFrame, YoVariableRegistry parentRegistry)
{
 this.allowMultipleFrames = allowMultipleFrames;
 YoVariableRegistry registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName());
 YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames(namePrefix + "ConstantPosition", registry, referenceFrame);
 position = yoFramePointInMultipleFrames;
 YoFrameQuaternionInMultipleFrames yoFrameQuaternionInMultiplesFrames = new YoFrameQuaternionInMultipleFrames(namePrefix + "ConstantOrientation",
    registry, referenceFrame);
 orientation = yoFrameQuaternionInMultiplesFrames;
 multipleFramesHolders = new ArrayList<YoMultipleFramesHolder>();
 multipleFramesHolders.add(yoFramePointInMultipleFrames);
 multipleFramesHolders.add(yoFrameQuaternionInMultiplesFrames);
}

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

public void setInitialPosition(double x, double y, double z)
{
 this.initialPosition.set(x, y, z);
}

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

private void setCornerPointsToNaN()
{
 for (int i = 0; i < entryCornerPoints.size(); i++)
   entryCornerPoints.get(i).setToNaN();
 for (int i = 0; i < exitCornerPoints.size(); i++)
   exitCornerPoints.get(i).setToNaN();
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout=300000)
public void testChangeToRegisteredFrame()
{
 YoVariableRegistry registry = new YoVariableRegistry("youhou");
 YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames("blop", registry, new ReferenceFrame[]{worldFrame, frameA});
 yoFramePointInMultipleFrames.switchCurrentReferenceFrame(worldFrame);
 
 FramePoint3D framePoint = new FramePoint3D(worldFrame);
 Point3D point = RandomGeometry.nextPoint3D(random, 100.0, 100.0, 100.0);
 
 yoFramePointInMultipleFrames.set(point);
 framePoint.set(point);
 
 yoFramePointInMultipleFrames.changeFrame(frameA);
 framePoint.changeFrame(frameA);
 
 assertTrue(framePoint.epsilonEquals(yoFramePointInMultipleFrames, 1e-10));
 
 try
 {
   yoFramePointInMultipleFrames.changeFrame(frameB);
   fail("Should have thrown an exception");
 }
 catch (Exception e)
 {
   // Good
 }
}

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

initialPosition = new YoFramePointInMultipleFrames(namePrefix + "InitialPosition", registry, referenceFrame);
finalPosition = new YoFramePointInMultipleFrames(namePrefix + "FinalPosition", registry, referenceFrame);
currentPosition = new YoFramePointInMultipleFrames(namePrefix + "CurrentPosition", registry, referenceFrame);
currentVelocity = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentVelocity", registry, referenceFrame);
currentAcceleration = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentAcceleration", registry, referenceFrame);
     initialPosition.buildUpdatedYoFramePointForVisualizationOnly(), initialOrientationForViz, 0.1);
  final YoGraphicCoordinateSystem finalPoseViz = new YoGraphicCoordinateSystem(namePrefix + "FinalPose",
     finalPosition.buildUpdatedYoFramePointForVisualizationOnly(), finalOrientationForViz, 0.1);
  final YoGraphicCoordinateSystem currentPoseViz = new YoGraphicCoordinateSystem(namePrefix + "CurrentPose",
     currentPosition.buildUpdatedYoFramePointForVisualizationOnly(), currentOrientationForViz, 0.25);
  yoGraphicsList = new YoGraphicsList(namePrefix + "StraightLineTrajectory");
  yoGraphicsList.add(currentPositionViz);

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

YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames("blop", registry, worldFrame);
assertEquals(1, yoFramePointInMultipleFrames.getNumberOfReferenceFramesRegistered());
yoFramePointInMultipleFrames.getRegisteredReferenceFrames(referenceFrames);
  yoFramePointInMultipleFrames.registerReferenceFrame(worldFrame);
yoFramePointInMultipleFrames.registerReferenceFrame(frameA);
assertEquals(2, yoFramePointInMultipleFrames.getNumberOfReferenceFramesRegistered());
yoFramePointInMultipleFrames.getRegisteredReferenceFrames(referenceFrames);

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout=300000)
  public void testSetIncludingFrame()
  {
   YoVariableRegistry registry = new YoVariableRegistry("youhou");
   YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames("blop", registry, new ReferenceFrame[]{worldFrame, frameA});
   yoFramePointInMultipleFrames.switchCurrentReferenceFrame(worldFrame);
   
   FramePoint3D framePoint = EuclidFrameRandomTools.nextFramePoint3D(random, frameA, -100.0, 100.0, -100.0, 100.0, -100.0, 100.0);
   
   yoFramePointInMultipleFrames.setIncludingFrame(framePoint);
   assertTrue(framePoint.epsilonEquals(yoFramePointInMultipleFrames, 1e-10));
   
   try
   {
     yoFramePointInMultipleFrames.setIncludingFrame(new FramePoint3D(frameC));
     fail("Should have thrown an exception");
   }
   catch (Exception e)
   {
     // Good
   }
  }
}

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

public void initialize()
{
 tangentialPlane.update();
 currentTrajectoryFrame = initialPosition.getReferenceFrame();
 changeFrame(tangentialPlane, false);
 currentTime.set(0.0);
 MathTools.checkIfInRange(trajectoryTime.getDoubleValue(), 0.0, Double.POSITIVE_INFINITY);
 double tIntermediate = leaveTime.getDoubleValue();
 xyPolynomial.setCubic(tIntermediate, trajectoryTime.getDoubleValue(), 0.0, 0.0, 1.0, 0.0);
 double z0 = initialPosition.getZ();
 double zIntermediate = initialPosition.getZ() + leaveDistance.getDoubleValue();
 double zf = finalPosition.getZ();
 zPolynomial.setSexticUsingWaypoint(0.0, tIntermediate, trajectoryTime.getDoubleValue(), z0, 0.0, 0.0, zIntermediate, zf, 0.0, 0.0);
 currentPosition.set(initialPosition);
 currentVelocity.setToZero();
 currentAcceleration.setToZero();
 changeFrame(currentTrajectoryFrame, false);
 if (visualize)
   visualizeTrajectory();
}

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

YoFramePointInMultipleFrames entryConstantCMP = new YoFramePointInMultipleFrames(namePrefix + "EntryCMP" + i, parentRegistry, framesToRegister);
entryConstantCMP.setToNaN();
entryCMPs.add(entryConstantCMP);
entryCMPsInWorldFrameReadOnly.add(entryConstantCMP.buildUpdatedYoFramePointForVisualizationOnly());
YoFramePointInMultipleFrames exitConstantCMP = new YoFramePointInMultipleFrames(namePrefix + "ExitCMP" + i, parentRegistry, framesToRegister);
exitConstantCMP.setToNaN();
exitCMPs.add(exitConstantCMP);
exitCMPsInWorldFrameReadOnly.add(exitConstantCMP.buildUpdatedYoFramePointForVisualizationOnly());

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

public YoFramePoint buildUpdatedYoFramePointForVisualizationOnly()
{
 if (yoFramePointInWorld == null)
 {
   final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
   if (!isReferenceFrameRegistered(worldFrame))
    registerReferenceFrame(worldFrame);
   yoFramePointInWorld = new YoFramePoint(namePrefix, worldFrame.getName(), worldFrame, registry);
   attachVariableChangedListener(new VariableChangedListener()
   {
    private final FramePoint localFramePoint = new FramePoint();
    private final YoFramePoint point = yoFramePointInWorld;
    @Override
    public void variableChanged(YoVariable<?> v)
    {
      getFrameTupleIncludingFrame(localFramePoint);
      point.setAndMatchFrame(localFramePoint);
    }
   });
 }
 return yoFramePointInWorld;
}

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

@Override
  public String toString()
  {
   String ret = "";

   ReferenceFrame currentFrame = initialPosition.getReferenceFrame();

   ret += "Current time: " + currentTime.getDoubleValue() + ", trajectory time: " + trajectoryTime.getDoubleValue();
   ret += "\nCurrent position: " + currentPosition.toStringForASingleReferenceFrame(currentFrame);
   ret += "\nCurrent velocity: " + currentVelocity.toStringForASingleReferenceFrame(currentFrame);
   ret += "\nCurrent acceleration: " + currentAcceleration.toStringForASingleReferenceFrame(currentFrame);
   return ret;
  }
}

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

private void computeFinalCMPBetweenSupportFeet(int cmpIndex, FrameConvexPolygon2d footA, FrameConvexPolygon2d footB)
{
 footA.getCentroid(tempCentroid);
 firstCMP.setXYIncludingFrame(tempCentroid);
 firstCMP.changeFrame(worldFrame);
 footB.getCentroid(tempCentroid);
 secondCMP.setXYIncludingFrame(tempCentroid);
 secondCMP.changeFrame(worldFrame);
 upcomingSupport.clear(worldFrame);
 tempFootPolygon.setIncludingFrame(footA);
 tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame);
 upcomingSupport.addVertices(tempFootPolygon);
 tempFootPolygon.setIncludingFrame(footB);
 tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame);
 upcomingSupport.addVertices(tempFootPolygon);
 upcomingSupport.update();
 entryCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame);
 exitCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame);
 upcomingSupport.getCentroid(tempCentroid);
 tempCentroid3d.setXYIncludingFrame(tempCentroid);
 double chicken = MathTools.clipToMinMax(percentageChickenSupport.getDoubleValue(), 0.0, 1.0);
 if (chicken <= 0.5)
   entryCMPs.get(cmpIndex).interpolate(firstCMP, tempCentroid3d, chicken * 2.0);
 else
   entryCMPs.get(cmpIndex).interpolate(tempCentroid3d, secondCMP, (chicken-0.5) * 2.0);
 exitCMPs.get(cmpIndex).set(entryCMPs.get(cmpIndex));
}

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

public void setInitialPose(FramePose initialPose)
{
 initialPose.getPoseIncludingFrame(tempPosition, tempOrientation);
 tempPosition.changeFrame(initialPosition.getReferenceFrame());
 initialPosition.set(tempPosition);
 tempOrientation.changeFrame(initialOrientation.getReferenceFrame());
 initialOrientation.set(tempOrientation);
 initialOrientationForViz.setAndMatchFrame(tempOrientation);
}

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

@Override
public void getPosition(FramePoint positionToPack)
{
 currentPosition.getFrameTupleIncludingFrame(positionToPack);
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout=300000)
public void testSetToZero()
{
 YoVariableRegistry registry = new YoVariableRegistry("youhou");
 YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames("blop", registry, allFrames);
 yoFramePointInMultipleFrames.switchCurrentReferenceFrame(worldFrame);
 
 FramePoint3D framePoint = new FramePoint3D(worldFrame);
 framePoint.setToZero(worldFrame);
 assertTrue(framePoint.epsilonEquals(yoFramePointInMultipleFrames, 1e-10));
}

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

@Override
public void changeFrame(ReferenceFrame desiredFrame)
{
 get(point);
 ReferenceFrame currentReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(desiredFrame);
 framePoint.setIncludingFrame(currentReferenceFrame, point);
 framePoint.changeFrame(desiredFrame);
 framePoint.get(point);
 set(point);
}

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

List<YoFramePoint> exitCMPs = referenceCMPsCalculator.getExitCMPs();
switchCornerPointsToWorldFrame();
singleSupportInitialICP.switchCurrentReferenceFrame(worldFrame);
singleSupportFinalICP.switchCurrentReferenceFrame(worldFrame);
  singleSupportInitialICP.setIncludingFrame(actualICPToHold);
  singleSupportFinalICP.setIncludingFrame(actualICPToHold);
  singleSupportInitialICPVelocity.set(0.0, 0.0, 0.0);
  setCornerPointsToNaN();
  singleSupportInitialICP.setIncludingFrame(entryCMPs.get(0));
  singleSupportFinalICP.setIncludingFrame(singleSupportInitialICP);
  singleSupportInitialICPVelocity.set(0.0, 0.0, 0.0);
  setCornerPointsToNaN();
                 swingTimeSplitFraction.getDoubleValue(), transferTimeSplitFraction.getDoubleValue(), omega0);
   computeDesiredCapturePointPosition(omega0, timeToSpendOnExitCMPBeforeNextDoubleSupport, exitCornerPoints.get(1), exitCMPs.get(1), singleSupportFinalICP);
   exitCornerPoints.get(0).changeFrame(initialFrame);
   exitCornerPoints.get(1).changeFrame(finalFrame);
  entryCornerPoints.get(0).changeFrame(initialFrame);
  entryCornerPoints.get(1).changeFrame(finalFrame);
  changeFrameOfRemainingCornerPoints(2, worldFrame);
  isHoldingPosition.set(false);
singleSupportInitialICP.changeFrame(finalFrame);
singleSupportFinalICP.changeFrame(worldFrame);

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

entryCMPs.get(cmpIndex).setToNaN();
   entryCMPs.get(cmpIndex).setIncludingFrame(cmp);
  exitCMPs.get(cmpIndex).setIncludingFrame(cmp);
  cmpIndex++;
entryCMPs.get(cmpIndex).setIncludingFrame(cmp);
firstEntryCMPForSingleSupport.setByProjectionOntoXYPlaneIncludingFrame(cmp);
computeFootstepCentroid(centroidOfUpcomingFootstep, upcomingFootsteps.get(0));
computeExitCMPForSupportFoot(cmp, transferToSide, centroidOfUpcomingFootstep, isUpcomingFootstepLast);
cmp.changeFrame(transferToSoleFrame);
exitCMPs.get(cmpIndex).setIncludingFrame(cmp);
cmpIndex++;

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

currentPosition.set(finalPosition);
currentVelocity.setToZero();
currentAcceleration.setToZero();
currentPosition.interpolate(initialPosition, finalPosition, quinticParameterPolynomial.getPosition());
currentVelocity.subAndScale(alphaVel, finalPosition, initialPosition);
currentAcceleration.subAndScale(alphaAcc, finalPosition, initialPosition);

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

singleSupportInitialICP.switchCurrentReferenceFrame(worldFrame);
singleSupportFinalICP.switchCurrentReferenceFrame(worldFrame);
  icpSingleSupportTrajectoryGenerator.initialize();
  exitCornerPoints.get(0).changeFrame(supportSoleFrame);
singleSupportInitialICP.changeFrame(supportSoleFrame);
entryCornerPoints.get(0).changeFrame(supportSoleFrame);
singleSupportFinalICP.changeFrame(worldFrame);
changeFrameOfRemainingCornerPoints(1, worldFrame);