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

YoFramePoint.add介绍

暂无

代码示例

代码示例来源：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/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 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

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

public void add(YoFramePose yoFramePose)
{
 getPosition().add(yoFramePose.getPosition());
 getOrientation().add(yoFramePose.getOrientation());
}

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

referenceAverageFootPosition.add(referenceFootPositions.get(foot));

代码示例来源：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/IHMCStateEstimation

copPositionsInWorld.get(trustedFoot).add(tempCoPOffset);

代码示例来源：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/IHMCHumanoidBehaviors

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

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

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