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

YoFrameVector.length介绍

暂无

代码示例

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

private double getStepTimeGivenWaypointSpeed(double waypointSpeed, double[] arcLengths)
{
 double initialSpeed = allVelocities[0].length();
 double finalSpeed = allVelocities[5].length();
 return 2 * arcLengths[0] / (initialSpeed + waypointSpeed) + (arcLengths[1] + arcLengths[2] + arcLengths[3]) / waypointSpeed
    + 2 * arcLengths[4] / (waypointSpeed + finalSpeed);
}

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

private void computeProportionalTerm(FramePoint desiredPosition)
{
 desiredPosition.changeFrame(bodyFrame);
 positionError.set(desiredPosition);
 // Limit the maximum position error considered for control action
 double maximumError = gains.getMaximumProportionalError();
 double errorMagnitude = positionError.length();
 positionError.getFrameTuple(proportionalTerm);
 if (errorMagnitude > maximumError)
 {
   proportionalTerm.scale(maximumError / errorMagnitude);
 }
 proportionalGainMatrix.transform(proportionalTerm.getVector());
}

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

@Override
public void initialize()
{
 currentTime.set(0.0);
 initialAngularVelocityMagnitude.set(initialAngularVelocity.length());
 finalAngularVelocityMagnitude.set(finalAngularVelocity.length());
 maxAngularVelocityMagnitudeAtLimits.set(PI / trajectoryTime.getDoubleValue());
 initialDriftSaturated.set(initialAngularVelocityMagnitude.getDoubleValue() > maxAngularVelocityMagnitudeAtLimits.getDoubleValue());
 finalDriftSaturated.set(finalAngularVelocityMagnitude.getDoubleValue() > maxAngularVelocityMagnitudeAtLimits.getDoubleValue());
 parameterPolynomial.setQuintic(0, trajectoryTime.getDoubleValue(), 0.0, 0.0, 0.0, 1.0, 0.0, 0.0);
 if (initialOrientation.dot(finalOrientation) < 0.0)
   finalOrientation.negate();
 currentOrientation.set(initialOrientation);
 currentAngularVelocity.set(initialAngularVelocity);
 currentAngularAcceleration.setToZero();
}

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

/**
* Computes the new derivative gains to use that reduces the amount of damping in the direction with the most position error.
* This prevents a high desired correcting velocity from being penalized when it deviates from the objective motion.
* @param positionError current position error being used by the feedback control
* @param derivativeGainsToPack derivative gain matrix to use with less damping in the main direction of motion
*/
public void compute(YoFrameVector positionError, Matrix3d derivativeGainsToPack)
{
 this.positionError.setIncludingFrame(positionError.getFrameTuple());
 if (positionError.length() > tangentialDampingGains.getParallelDampingDeadband())
 {
   bodyFrameTangentToControl.update();
   double alpha = computeDampingReductionRatioParallelToMotion(positionError.length());
   transformedGains.setToZero(bodyFrame);
   transformedGains.set(derivativeGainsToPack);
   transformedGains.changeFrame(bodyFrameTangentToControl);
   transformedGains.setElement(0, 0, alpha * transformedGains.getElement(0, 0));
   transformedGains.setElement(1, 0, alpha * transformedGains.getElement(1, 0));
   transformedGains.setElement(2, 0, alpha * transformedGains.getElement(2, 0));
   transformedGains.changeFrame(bodyFrame);
   transformedGains.getMatrix(derivativeGainsToPack);
 }
}

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

private void applyForce()
{      
 double length = pushDirection.length();
 if (length > 1e-5)
 {
   pushForce.set(pushDirection);
   pushForce.normalize();
   pushForce.scale(pushForceMagnitude.getDoubleValue());
   if(pushCondition == null)
   {
    pushTimeSwitch.set(yoTime.getDoubleValue());
   }
 }
 else
 {
   pushForce.setToZero();
   pushTimeSwitch.set(Double.NEGATIVE_INFINITY);
 }
 pushNumber.increment();
}

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

private void setWaypointAndAccelerationEndpointTimesAndVelocities(double[] arcLengths)
{
 double waypointSpeed = waypointsAreTheSamePoint ? 0.0 : getWaypointSpeed(arcLengths);
 for (int i = 0; i < 2; i++)
 {
   FrameVector waypointVelocity = getOppositeWaypointToEndpoint(i);
   waypointVelocity.normalize();
   waypointVelocity.scale(waypointSpeed * ((i == 0) ? -1 : 1));
   allVelocities[waypointIndices[i]].set(waypointVelocity);
   allVelocities[accelerationEndpointIndices[i]].set(waypointVelocity);
 }
 if (waypointsAreTheSamePoint)
 {
   double totalArcLength = getTotalArcLength(arcLengths);
   allTimes[1].set((arcLengths[0] + arcLengths[1]) / totalArcLength * stepTime.getDoubleValue());
   for (int i = 2; i < 5; i++)
   {
    allTimes[i].set(allTimes[1].getDoubleValue());
   }
 }
 else
 {
   allTimes[1].set(2 * arcLengths[0] / (allVelocities[0].length() + waypointSpeed));
   allTimes[2].set(allTimes[1].getDoubleValue() + arcLengths[1] / waypointSpeed);
   allTimes[3].set(allTimes[2].getDoubleValue() + arcLengths[2] / waypointSpeed);
   allTimes[4].set(allTimes[3].getDoubleValue() + arcLengths[3] / waypointSpeed);
 }
}