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

YoFrameVector.sub介绍

暂无

代码示例

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

public void initialize(FramePoint initialPosition, FrameVector initialVelocity, FramePoint 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/IHMCRoboticsToolkit

public void initialize(FramePoint initialPosition, FramePoint intermediatePosition, FramePoint finalPosition, double intermediateParameter)
{
 initialPosition.changeFrame(referenceFrame);
 intermediatePosition.changeFrame(referenceFrame);
 finalPosition.changeFrame(referenceFrame);
 final double q = intermediateParameter;
 MathTools.checkIfInRange(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/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/IHMCHumanoidBehaviors

private double computeFrameOrientationRelativeToWalkingPath(ReferenceFrame referenceFrame)
{
 this.walkPathVector.sub(this.targetLocation, robotYoPose.getPosition());
 fullRobotModel.updateFrames();
 FrameVector2d frameHeadingVector = new FrameVector2d(referenceFrame, 1.0, 0.0);
 frameHeadingVector.changeFrame(worldFrame);
 double ret = -frameHeadingVector.angle(walkPathVector.getFrameVector2dCopy());
 if (DEBUG)
 {
   PrintTools.debug(this, "FrameHeadingVector : " + frameHeadingVector);
   PrintTools.debug(this, "WalkPathVector : " + walkPathVector);
   PrintTools.debug(this, "OrientationToWalkPath : " + ret);
 }
 return ret;
}

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

public void update(Wrench sensorWrench)
{
 sensorWrench.changeFrame(world);
 yoSensorForce.set(sensorWrench.getExpressedInFrame(), sensorWrench.getLinearPartX(), sensorWrench.getLinearPartY(), sensorWrench.getLinearPartZ());
 yoSensorTorque.set(sensorWrench.getExpressedInFrame(), sensorWrench.getAngularPartX(), sensorWrench.getAngularPartY(), sensorWrench.getAngularPartZ());
 if (addSimulatedSensorNoise.getBooleanValue())
 {
   double amp = 0.1;
   double bias = 0.25;
      yoSensorForce.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);
   yoSensorTorque.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);
 }
 
 updateSensorPosition();
 updateCenterOfMass();
 yoSensorToDistalCoMvectorInWorld.sub(distalCoMInWorld, yoSensorPositionInWorld);
 distalMassWrench.setToZero(world);
 distalMassWrench.setUsingArm(world, distalMassForceInWorld.getFrameTuple().getVector(), yoSensorToDistalCoMvectorInWorld.getFrameTuple().getVector());
 yoSensorForceFromDistalMass.set(distalMassWrench.getExpressedInFrame(), distalMassWrench.getLinearPartX(), distalMassWrench.getLinearPartY(), distalMassWrench.getLinearPartZ());
 yoSensorTorqueFromDistalMass.set(distalMassWrench.getExpressedInFrame(), distalMassWrench.getAngularPartX(), distalMassWrench.getAngularPartY(), distalMassWrench.getAngularPartZ());
 yoSensorForceMassCompensated.sub(yoSensorForce, yoSensorForceFromDistalMass);
 yoSensorTorqueMassCompensated.sub(yoSensorTorque, yoSensorTorqueFromDistalMass);
}

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

public void updateIMUAndRootJointLinearVelocity(FrameVector rootJointVelocityToPack)
{
 updateLinearAccelerationMeasurement();
 yoLinearAccelerationMeasurementInWorld.getFrameTupleIncludingFrame(linearAccelerationMeasurement);
 linearAccelerationMeasurement.scale(estimatorDT);
 yoMeasurementFrameLinearVelocityInWorld.add(linearAccelerationMeasurement);
 yoMeasurementFrameLinearVelocityInWorld.getFrameTupleIncludingFrame(rootJointVelocityToPack);
 getCorrectionVelocityForMeasurementFrameOffset(correctionVelocityForMeasurementFrameOffset);
 correctionVelocityForMeasurementFrameOffset.changeFrame(worldFrame);
 rootJointVelocityToPack.sub(correctionVelocityForMeasurementFrameOffset);
 yoRootJointIMUBasedLinearVelocityInWorld.set(rootJointVelocityToPack);
 imuLinearVelocityIMUOnly.add(linearAccelerationMeasurement);
 imuLinearVelocityIMUOnly.scale(alphaLeakIMUOnly.getDoubleValue());
 rootJointLinearVelocityIMUOnly.set(imuLinearVelocityIMUOnly);
 rootJointLinearVelocityIMUOnly.sub(correctionVelocityForMeasurementFrameOffset);
}

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

@Override
public void compute(double time)
{
 if (continuouslyUpdateFinalPosition.getBooleanValue())
 {
   updateFinalPosition();
 }
 this.currentTime.set(time);
 time = MathTools.clipToMinMax(time, 0.0, trajectoryTime.getDoubleValue());
 parameterPolynomial.compute(time);
 differenceVector.sub(finalPosition, initialPosition);
 double parameter = isDone() ? 1.0 : parameterPolynomial.getPosition();
 double parameterd = isDone() ? 0.0 : parameterPolynomial.getVelocity();
 double parameterdd = isDone() ? 0.0 : parameterPolynomial.getAcceleration();
 currentPosition.interpolate(initialPosition, finalPosition, parameter);
 currentVelocity.set(differenceVector);
 currentVelocity.scale(parameterd);
 currentAcceleration.set(differenceVector);
 currentAcceleration.scale(parameterdd);
}

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

adjustmentVelocityCorrection.sub(unadjustedPosition);
adjustmentVelocityCorrection.scale(1.0 / controlDT);
adjustmentVelocityCorrection.setZ(0.0);