public double lengthSquared() { return getFrameTuple2d().lengthSquared(); }
boolean icpErrorSmall = icpError.lengthSquared() < maxIcpError.getDoubleValue() * maxIcpError.getDoubleValue();
private void getTransformedFeedbackGains(FrameVector2d feedbackGainsToPack) { double epsilonZeroICPVelocity = 1e-5; if (desiredICPVelocity.lengthSquared() > MathTools.square(epsilonZeroICPVelocity)) { icpVelocityDirectionFrame.setXAxis(desiredICPVelocity); RigidBodyTransform transform = icpVelocityDirectionFrame.getTransformToWorldFrame(); transform.getRotation(rotation); rotationTranspose.set(rotation); rotationTranspose.transpose(); transformTranspose.setRotation(rotationTranspose); gainsMatrix.setZero(); gainsMatrix.setElement(0, 0, 1.0 + feedbackParallelGain.getDoubleValue()); gainsMatrix.setElement(1, 1, 1.0 + feedbackOrthogonalGain.getDoubleValue()); gainsMatrixTransformed.set(rotation); gainsMatrixTransformed.mul(gainsMatrix); gainsMatrixTransformed.mul(rotationTranspose); feedbackGainsToPack.setToZero(worldFrame); feedbackGainsToPack.setX(gainsMatrixTransformed.getElement(0, 0)); feedbackGainsToPack.setY(gainsMatrixTransformed.getElement(1, 1)); } else { feedbackGainsToPack.setToZero(worldFrame); feedbackGainsToPack.set(feedbackOrthogonalGain.getDoubleValue(), feedbackOrthogonalGain.getDoubleValue()); } yoFeedbackGains.set(feedbackGainsToPack); }
if (desiredCapturePointVelocity.lengthSquared() > MathTools.square(epsilonZeroICPVelocity))