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

YoFramePoint介绍

暂无

代码示例

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

@Override
public void setPosition(Point3d position)
{
 this.position.set(position);
}

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

public YoSE3ConfigurationProvider(String name, ReferenceFrame frame, YoVariableRegistry registry)
{
 position = new YoFramePoint(name, frame, registry);
 orientation = new YoFrameQuaternion(name, frame, registry);
}

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

@Override
public void getPosition(Point3d positionToPack)
{
 position.get(positionToPack);
}

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

/**
* Put the constant CMP in the middle of the two given footsteps.
* @param constantCMPToPack YoFramePoint that will be packed with the constant CMP location
* @param firstFootstep FramePoint holding the position of the first footstep
* @param secondFootstep FramePoint holding the position of the second footstep
*/
public static void putConstantCMPBetweenFeet(YoFramePoint constantCMPToPack, FramePoint firstFootstep, FramePoint secondFootstep)
{
 constantCMPToPack.setAndMatchFrame(firstFootstep);
 constantCMPToPack.add(secondFootstep);
 constantCMPToPack.scale(0.5);
}

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

public double distance(YoFramePoint yoFramePoint)
{
 return distance(yoFramePoint.getFrameTuple());
}

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

private void setupSlidersForSupportBaseTargetControl()
{
 int sliderChannel = 1;
 for (int i = 0; i < baseControlTargetPoints.length; i++)
 {
   YoFramePoint baseControlTargetPoint = baseControlTargetPoints[i];
   sliderBoard.setSlider(sliderChannel++, baseControlTargetPoint.getYoX(), baseControlTargetPoint.getX() - 2, baseControlTargetPoint.getX() + 2);
   sliderBoard.setSlider(sliderChannel++, baseControlTargetPoint.getYoY(), baseControlTargetPoint.getY() - 2, baseControlTargetPoint.getY() + 2);
 }
}

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

private void updateTrustedFootPosition(RigidBody trustedFoot)
{
 YoFramePoint footPositionInWorld = footPositionsInWorld.get(trustedFoot);
 AlphaFilteredYoFrameVector footToRootJointPosition = footToRootJointPositions.get(trustedFoot);
 if (trustCoPAsNonSlippingContactPoint.getBooleanValue())
 {
   footToRootJointPosition.getFrameTupleIncludingFrame(tempPosition);
   rootJointPosition.getFrameTupleIncludingFrame(tempFramePoint);
   tempFramePoint.sub(tempPosition); // New foot position
   footPositionInWorld.getFrameTuple(tempPosition); // Previous foot position
   tempFrameVector.sub(tempFramePoint, tempPosition); // Delta from previous to new foot position
   copPositionsInWorld.get(trustedFoot).add(tempFrameVector); // New CoP position
 }
 footPositionInWorld.set(rootJointPosition);
 footPositionInWorld.sub(footToRootJointPosition);
}

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

@Override
public void initialize()
{
 currentTime.set(0.0);
 double tIntermediate = trajectoryTime.getDoubleValue() / 2.0;
 xPolynomial.setQuadraticWithFinalVelocityConstraint(0.0,trajectoryTime.getDoubleValue(), initialPosition.getX(), finalPosition.getX(), finalVelocity.getX());
 yPolynomial.setQuadraticWithFinalVelocityConstraint(0.0,trajectoryTime.getDoubleValue(), initialPosition.getY(), finalPosition.getY(), finalVelocity.getY());
 zPolynomial.setCubicWithIntermediatePositionAndFinalVelocityConstraint(0.0,  tIntermediate, trajectoryTime.getDoubleValue(), initialPosition.getZ(), intermediateZPosition.getDoubleValue(), finalPosition.getZ(), finalVelocity.getZ());
 currentPosition.set(initialPosition);
 currentAcceleration.setToZero();
}

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

@Override
protected void updateTransformToParent(RigidBodyTransform transformToParent)
{
 yoFramePose.getOrientation().getQuaternion(rotation);
 transformToParent.setRotation(rotation);
 YoFramePoint yoFramePoint = yoFramePose.getPosition();
 transformToParent.setTranslation(yoFramePoint.getX(), yoFramePoint.getY(), yoFramePoint.getZ());
}

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

FramePoint origin = new FramePoint(ReferenceFrame.getWorldFrame(), 0.5, -1.0, 2.0);
vertices.set(RobotQuadrant.FRONT_LEFT, new YoFramePoint(RobotQuadrant.FRONT_LEFT.getCamelCaseNameForStartOfExpression(), ReferenceFrame.getWorldFrame(), registry));
vertices.set(RobotQuadrant.FRONT_RIGHT, new YoFramePoint(RobotQuadrant.FRONT_RIGHT.getCamelCaseNameForStartOfExpression(), ReferenceFrame.getWorldFrame(), registry));
vertices.set(RobotQuadrant.HIND_RIGHT, new YoFramePoint(RobotQuadrant.HIND_RIGHT.getCamelCaseNameForStartOfExpression(), ReferenceFrame.getWorldFrame(), registry));
vertices.set(RobotQuadrant.HIND_LEFT, new YoFramePoint(RobotQuadrant.HIND_LEFT.getCamelCaseNameForStartOfExpression(), ReferenceFrame.getWorldFrame(), registry));
vertices.get(RobotQuadrant.FRONT_LEFT).set(0.0, 1.0, 0.0);
vertices.get(RobotQuadrant.FRONT_RIGHT).set(1.0, 1.0, 0.0);
vertices.get(RobotQuadrant.HIND_RIGHT).set(1.0, 0.0, 0.0);
vertices.get(RobotQuadrant.HIND_LEFT).set(0.0, 0.0, 0.0);
vertices.get(RobotQuadrant.FRONT_LEFT).add(origin);
vertices.get(RobotQuadrant.FRONT_RIGHT).add(origin);
vertices.get(RobotQuadrant.HIND_RIGHT).add(origin);
vertices.get(RobotQuadrant.HIND_LEFT).add(origin);
vertices.get(RobotQuadrant.FRONT_LEFT).set(tempCommonSupportPolygon.getFootstep(RobotQuadrant.FRONT_LEFT));
vertices.get(RobotQuadrant.FRONT_RIGHT).set(tempCommonSupportPolygon.getFootstep(RobotQuadrant.FRONT_RIGHT));
vertices.remove(RobotQuadrant.HIND_RIGHT);
vertices.get(RobotQuadrant.HIND_LEFT).set(tempCommonSupportPolygon.getFootstep(RobotQuadrant.HIND_LEFT));

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

private void updateCurrents()
{
 if (numberOfFeetTrusted.getIntegerValue() == numberOfFeet)
 {
   for (int i = 0; i < numberOfFeet; i++)
   {
    RigidBody foot = allFeet.get(i);
    footPosition.setToZero(footSoleFrames.get(foot));
    currentFootPositions.get(foot).setAndMatchFrame(footPosition);
   }
   currentAverageFootPosition.setToZero();
   for (int i = 0; i < numberOfFeet; i++)
   {
    RigidBody foot = allFeet.get(i);
    currentAverageFootPosition.add(currentFootPositions.get(foot));
   }
   currentAverageFootPosition.scale(1.0 / numberOfFeet);
 }
 else
 {
   for (int i = 0; i < numberOfFeet; i++)
    currentFootPositions.get(allFeet.get(i)).setToNaN();
   currentAverageFootPosition.setToNaN();
 }
}

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

/**
* Estimates the pelvis position and linear velocity using the leg kinematics
* @param trustedFoot is the foot used to estimates the pelvis state
* @param numberOfTrustedSides is only one or both legs used to estimate the pelvis state
*/
private void updatePelvisWithKinematics(RigidBody trustedFoot, int numberOfTrustedFeet)
{
 double scaleFactor = 1.0 / numberOfTrustedFeet;
 footToRootJointPositions.get(trustedFoot).getFrameTuple(tempPosition);
 tempPosition.scale(scaleFactor);
 rootJointPosition.add(tempPosition);
 footPositionsInWorld.get(trustedFoot).getFrameTuple(tempPosition);
 tempPosition.scale(scaleFactor);
 rootJointPosition.add(tempPosition);
 YoFramePoint rootJointPositionPerFoot = rootJointPositionsPerFoot.get(trustedFoot);
 rootJointPositionPerFoot.set(footPositionsInWorld.get(trustedFoot));
 rootJointPositionPerFoot.add(footToRootJointPositions.get(trustedFoot));
 footVelocitiesInWorld.get(trustedFoot).getFrameTupleIncludingFrame(tempFrameVector);
 tempFrameVector.scale(scaleFactor * alphaRootJointLinearVelocityNewTwist.getDoubleValue());
 rootJointLinearVelocityNewTwist.sub(tempFrameVector);
}

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

private void computePositionFromMergingMeasurements()
{
 yoRootJointPosition.getFrameTuple(rootJointPosition);
 imuBasedLinearStateCalculator.updatePelvisPosition(rootJointPosition, pelvisPositionIMUPart);
 kinematicsBasedLinearStateCalculator.getPelvisPosition(pelvisPositionKinPart);
 pelvisPositionIMUPart.scale(alphaIMUAgainstKinematicsForPosition.getDoubleValue());
 pelvisPositionKinPart.scale(1.0 - alphaIMUAgainstKinematicsForPosition.getDoubleValue());
 rootJointPosition.set(pelvisPositionIMUPart);
 rootJointPosition.add(pelvisPositionKinPart);
 yoRootJointPosition.set(rootJointPosition);
}

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

public void update()
{
 if (footRawCoPPositionsInWorld != null)
 {
   overallRawCoPPositionInWorld.setToZero();
   double totalFootForce = 0.0;
   for (int i = 0; i < footList.size(); i++)
   {
    RigidBody rigidBody = footList.get(i);
    footSwitches.get(rigidBody).computeAndPackCoP(tempRawCoP2d);
    tempRawCoP.setIncludingFrame(tempRawCoP2d.getReferenceFrame(), tempRawCoP2d.getX(), tempRawCoP2d.getY(), 0.0);
    tempRawCoP.changeFrame(worldFrame);
    footRawCoPPositionsInWorld.get(rigidBody).set(tempRawCoP);
    footSwitches.get(rigidBody).computeAndPackFootWrench(tempWrench);
    double singleFootForce = tempWrench.getLinearPartZ();
    totalFootForce += singleFootForce;
    tempRawCoP.scale(singleFootForce);
    overallRawCoPPositionInWorld.add(tempRawCoP);
   }
   overallRawCoPPositionInWorld.scale(1.0 / totalFootForce);
 }
}

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

quadrupedSupportPolygon.setFootstep(quadrant, yoFootPositions.get(quadrant).getFrameTuple());
  cartesianTrajectoryGenerator.getPosition(swingLegPosition);
  yoFootPositions.get(robotQuadrant).set(swingLegPosition);
  swingLeg.set(robotQuadrant);
  FramePoint initialPosition = new FramePoint(yoFootPositions.get(robotQuadrant).getFramePointCopy());
  FramePoint desiredFootPosition = new FramePoint();
  cartesianTrajectoryGenerator.setTrajectoryParameters(desiredSwingTime.getDoubleValue(), initialPosition, swingHeight.getDoubleValue(), desiredFootPosition, finalDesiredVelocity.getFrameTuple());
  cartesianTrajectoryGenerator.initialize();
  swingTarget.set(desiredFootPosition);
  FramePoint footPosition = yoFootPosition.getFramePointCopy();
  footPosition.changeFrame(ReferenceFrame.getWorldFrame());
  fourFootSupportPolygon.setFootstep(robotQuadrant, footPosition);
centroid.set(temporaryCentroid);
nominalYaw.set(fourFootSupportPolygon.getNominalYaw());
endPoint.add(0.4,0.0,0.0);
endPoint.yawAboutPoint(temporaryCentroid, endPoint, nominalYaw.getDoubleValue());
nominalYawEndpoint.set(endPoint);
nominalYawLineSegment.set(centroid.getFramePoint2dCopy(), endpointTwoD);
nominalYawGraphic.update();

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

private void setAccelerationEndpointPositions()
{
 for (int i : new int[] {0, 1})
 {
   FrameVector waypointToEndpoint = getWaypointToEndpoint(i);
   FrameVector oppositeWaypointToEndpoint = getOppositeWaypointToEndpoint(i);
   double scaleFactor = waypointToEndpoint.dot(oppositeWaypointToEndpoint) / oppositeWaypointToEndpoint.length()
      * linearSplineLengthFactor.getDoubleValue();
   oppositeWaypointToEndpoint.normalize();
   oppositeWaypointToEndpoint.scale(scaleFactor);
   allPositions[accelerationEndpointIndices[i]].set(allPositions[waypointIndices[i]].getFramePointCopy());
   allPositions[accelerationEndpointIndices[i]].add(oppositeWaypointToEndpoint);
 }
}

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

/**
* GC-free but unsafe accessor.
*/
public FramePoint getPosition()
{
 return position.getFrameTuple();
}

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

protected void checkForCloseWaypoints()
{
 if (waypointsAreCloseTogether())
 {
   FramePoint midpoint = allPositions[waypointIndices[0]].getFramePointCopy();
   midpoint.add(allPositions[waypointIndices[1]].getFramePointCopy());
   midpoint.scale(0.5);
   for (int i = 0; i < 2; i++)
   {
    allPositions[waypointIndices[i]].set(midpoint);
   }
   waypointsAreTheSamePoint = true;
 }
 else
 {
   waypointsAreTheSamePoint = false;
 }
}

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

public void initialize()
{
 time.set(0.0);
 FramePoint positionToPack = new FramePoint();
 positionProvider.getPosition(positionToPack);
 positionToPack.changeFrame(position.getReferenceFrame());
 position.set(positionToPack);
}

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

public void update(FramePoint forcePointPosition, FrameVector force)
 yoForcePointPosition.set(forcePointPosition);
   yoForcePointPosition.add(amp * 2.0 * (Math.random() - 0.5) + bias, amp * 2.0 * (Math.random() - 0.5) + bias, amp * 2.0 * (Math.random() - 0.5) + bias);