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

YoFrameVector.add介绍

暂无

代码示例

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

public void corrupt(Tuple3d signal)
{
 double std = standardDeviation.getDoubleValue();
 double biasUpdateX = std * random.nextGaussian() * squareRootOfUpdateDT;
 double biasUpdateY = std * random.nextGaussian() * squareRootOfUpdateDT;
 double biasUpdateZ = std * random.nextGaussian() * squareRootOfUpdateDT;
 biasYoFrameVector.add(biasUpdateX, biasUpdateY, biasUpdateZ);
 biasYoFrameVector.get(biasVector);
 signal.add(biasVector);
}

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

public void correctIMULinearVelocity(FrameVector rootJointVelocity)
{
 rootJointLinearVelocity.set(rootJointVelocity);
 yoMeasurementFrameLinearVelocityInWorld.set(rootJointVelocity);
 yoMeasurementFrameLinearVelocityInWorld.add(correctionVelocityForMeasurementFrameOffset);
}

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

public void updatePelvisPosition(FramePoint rootJointPositionPrevValue, FramePoint rootJointPositionToPack)
{
 if (!isEstimationEnabled())
   throw new RuntimeException("IMU estimation module for pelvis linear velocity is disabled.");
 rootJointLinearVelocity.getFrameTupleIncludingFrame(tempRootJointVelocityIntegrated);
 tempRootJointVelocityIntegrated.scale(estimatorDT);
 if(setRootJointPositionImuOnlyToCurrent.getBooleanValue())
 {
   rootJointPositionImuOnly.set(rootJointPositionPrevValue);
   setRootJointPositionImuOnlyToCurrent.set(false);
 }
 rootJointPositionImuOnly.add(tempRootJointVelocityIntegrated);
 rootJointPosition.set(rootJointPositionPrevValue);
 rootJointPosition.add(tempRootJointVelocityIntegrated);
 rootJointPosition.getFrameTupleIncludingFrame(rootJointPositionToPack);
}

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

private void computeTotalGroundReactionForce()
{
 totalGroundReactionForce.setToZero();
 for (int i = 0; i < feet.size(); i++)
 {
   RigidBody foot = feet.get(i);
   Wrench footWrench = footWrenches.get(foot);
   footSwitches.get(foot).computeAndPackFootWrench(footWrench);
   footWrench.getLinearPartIncludingFrame(tempFootForce);
   tempFootForce.changeFrame(worldFrame);
   totalGroundReactionForce.add(tempFootForce);
 }
 totalGroundReactionForce.getFrameTuple(tempCoMAcceleration);
 comAcceleration.set(tempCoMAcceleration);
 comAcceleration.setZ(comAcceleration.getZ() - robotMass.getDoubleValue() * gravitationalAcceleration);
}

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

private void updateAngularVelocity()
{
 firstIMU.getAngularVelocityMeasurement(firstVector);
 secondIMU.getAngularVelocityMeasurement(secondVector);
 firstFrameVector.setIncludingFrame(firstIMU.getMeasurementFrame(), firstVector);
 secondFrameVector.setIncludingFrame(secondIMU.getMeasurementFrame(), secondVector);
 firstFrameVector.changeFrame(fusedMeasurementFrame);
 secondFrameVector.changeFrame(fusedMeasurementFrame);
 angularVelocity.add(firstFrameVector, secondFrameVector);
 angularVelocity.scale(0.5);
}

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

private void updateLinearAcceleration()
{
 firstIMU.getLinearAccelerationMeasurement(firstVector);
 secondIMU.getLinearAccelerationMeasurement(secondVector);
 firstFrameVector.setIncludingFrame(firstIMU.getMeasurementFrame(), firstVector);
 secondFrameVector.setIncludingFrame(secondIMU.getMeasurementFrame(), secondVector);
 firstFrameVector.changeFrame(fusedMeasurementFrame);
 secondFrameVector.changeFrame(fusedMeasurementFrame);
 linearAcceleration.add(firstFrameVector, secondFrameVector);
 linearAcceleration.scale(0.5);
}

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

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);
 }
 positionErrorCumulated.getFrameTuple(integralTerm);
 integralGainMatrix.transform(integralTerm.getVector());
}

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

private void computeIntegralTerm()
{
 if (gains.getMaximumIntegralError() < 1e-5)
 {
   integralTerm.setToZero(bodyFrame);
   return;
 }
 double integratedErrorAngle = errorAngleAxis.getAngle() * dt;
 double errorIntegratedX = errorAngleAxis.getX() * integratedErrorAngle;
 double errorIntegratedY = errorAngleAxis.getY() * integratedErrorAngle;
 double errorIntegratedZ = errorAngleAxis.getZ() * integratedErrorAngle;
 rotationErrorCumulated.add(errorIntegratedX, errorIntegratedY, errorIntegratedZ);
 double errorMagnitude = rotationErrorCumulated.length();
 if (errorMagnitude > gains.getMaximumIntegralError())
 {
   rotationErrorCumulated.scale(gains.getMaximumIntegralError() / errorMagnitude);
 }
 rotationErrorCumulated.getFrameTuple(integralTerm);
 integralGainMatrix.transform(integralTerm.getVector());
}

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

private void computeIntegralTerm(ReferenceFrame baseFrame)
{
 if (gains.getMaximumIntegralError() < 1e-5)
 {
   integralTerm.setToZero(baseFrame);
   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);
 }
 positionErrorCumulated.getFrameTupleIncludingFrame(integralTerm);
 integralTerm.changeFrame(baseFrame);
 integralGainMatrix.transform(integralTerm.getVector());
}

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

yoCenterOfMassVelocityIntegrateGRF.add(comAcceleration);
yoCenterOfMassVelocity.add(comVelocityGRFPart, comVelocityPelvisAndKinPart);

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

desiredPelvisAngularVelocity.add(tempAngularVelocity);
desiredPelvisAngularAcceleration.add(tempAngularAcceleration);

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

controlAngularVelocities[2].add(tempAngularVelocity);