private void computeDerivativeTerm(FrameVector desiredVelocity, FrameVector currentVelocity) { desiredVelocity.changeFrame(bodyFrame); currentVelocity.changeFrame(bodyFrame); derivativeTerm.sub(desiredVelocity, currentVelocity); // Limit the maximum velocity error considered for control action double maximumVelocityError = gains.getMaximumDerivativeError(); double velocityErrorMagnitude = derivativeTerm.length(); if (velocityErrorMagnitude > maximumVelocityError) { derivativeTerm.scale(maximumVelocityError / velocityErrorMagnitude); } velocityError.set(derivativeTerm); tempMatrix.set(derivativeGainMatrix); tangentialDampingCalculator.compute(positionError, tempMatrix); tempMatrix.transform(derivativeTerm.getVector()); }