Package tagalong.math

Class AlgebraicUtils

java.lang.Object
tagalong.math.AlgebraicUtils
public class AlgebraicUtils extends Object
Algebraic functions

  • Constructor Details

  • Method Details

    • cart2pol

      public static Pair<Double,Double> cart2pol(double x, double y)
      Parameters:
      x - Cartesian x coordinate
      y - Cartesian y coordinate
      Returns:
      pair representing the polar coordinate radius and theta in radians

    • pol2cart

      public static Pair<Double,Double> pol2cart(double r, double theta)
      Converts polar coordinates to Cartesian coordinates.
      Parameters:
      r - radius in polar coordinates
      theta - angle in radians
      Returns:
      pair of Cartesian coordinates x and y

    • constrainAngleNegPiToPi

      public static double constrainAngleNegPiToPi(double angle)
      Constrains an angle to be within [-pi, pi).
      Parameters:
      angle - angle in radians
      Returns:
      angle in radians that is between [-pi,pi]

    • placeInScopeRad

      public static double placeInScopeRad(double referenceAngleRad, double desiredAngleRad)
      Scopes the angle to be at most pi radians away from the reference angle
      Parameters:
      referenceAngleRad - Base of the scope to place the angle in
      desiredAngleRad - Angle that should be placed in radians
      Returns:
      |angle| placed within within [-pi, pi) of |referenceAngle|.

    • placeInScopeDeg

      public static double placeInScopeDeg(double referenceAngleDeg, double desiredAngleDeg)
      Scopes the angle to be at most 180 degrees away from the reference angle
      Parameters:
      referenceAngleDeg - Base of the scope to place the angle in
      desiredAngleDeg - Angle that should be placed in degrees
      Returns:
      |angle| placed within within [-180, 180) of |referenceAngle|.

    • placeInScopeRot

      public static double placeInScopeRot(double referenceAngleRot, double desiredAngleRot)
      Returns the desired angle in the closest scope of the current angle
      Parameters:
      referenceAngleRot - Current angle in rotations
      desiredAngleRot - Target angle in rotations
      Returns:
      Scoped angle in rotations

    • rotateVector

      public static Matrix<N2,N1> rotateVector(Matrix<N2,N1> vector, double theta)
      Rotates a vector by a given angle.
      Parameters:
      vector - initial vector
      theta - desired angle rotation
      Returns:
      rotated vector as a matrix

    • extractTFMatrixZ

      public static double extractTFMatrixZ(Matrix<N4,N4> matrix)
      Returns double z component from the 4*4 matrix.
      Parameters:
      matrix - 4*4 Matrix
      Returns:
      double Z component from the matrix

    • extractTFMatrixYRot

      public static double extractTFMatrixYRot(Matrix<N4,N4> matrix)
      Returns double Y rotation form the 4*4 matrix.
      Parameters:
      matrix - 4*4 matrix
      Returns:
      double Y rotation from the matrix

    • pose2DToZRotTFMatrix

      public static Matrix<N4,N4> pose2DToZRotTFMatrix(Pose2d pose)
      Converts the 2D pose into a matrix with a rotation around the z-axis.
      Parameters:
      pose - 2D position of the robot on the field
      Returns:
      Transformation matrix from Pose2d

    • zRotTFMatrixToPose

      public static Pose2d zRotTFMatrixToPose(Matrix<N4,N4> zRotTFMatrix)
      Converts matrix back into 2D pose.
      Parameters:
      zRotTFMatrix - a 4x4 matrix representing a 2D affine transformation with translation and rotation
      Returns:
      Pose2d

    • getPointInFrame

      public static Matrix<N3,N1> getPointInFrame(Matrix<N3,N1> point, Matrix<N4,N4> frame)
      Transforms a 3D point into a given frame of reference using matrix operations.
      Parameters:
      point - 3x1 column matrix representing the coordinates of a point. ---not sure if this is right
      frame - 4x4 matrix representing the pose
      Returns:
      Transformed point as matrix

    • makeYRotTFMatrix

      public static Matrix<N4,N4> makeYRotTFMatrix(double x, double y, double z, double theta)
      Creates a matrix about with rotation about the y-axis.
      Parameters:
      x - translation along x-axis
      y - translation along y-axis
      z - translation along z-axis
      theta - angle of rotation about y-axis
      Returns:
      transformation matrix for Y rotation

    • makeZRotTFMatrix

      public static Matrix<N4,N4> makeZRotTFMatrix(double x, double y, double z, double theta)
      Parameters:
      x - translation along x-axis
      y - translation along y-axis
      z - translation along z-axis
      theta - angle of rotation about Z-axis
      Returns:
      Transformation matrix for Z rotation

    • exp

      public static Pose2d exp(Twist2d delta)
      Obtain a new Pose2d from a (constant curvature) velocity. See: https://github.com/strasdat/Sophus/blob/master/sophus/se2.hpp. Borrowed from 254: https://github.com/Team254/FRC-2022-Public/blob/main/src/main/java/com/team254/lib/geometry/Pose2d.java
      Parameters:
      delta - robot change
      Returns:
      New Pose2d from velocity

    • log

      public static Twist2d log(Pose2d transform)
      Logical inverse of the above. Borrowed from 254: https://github.com/Team254/FRC-2022-Public/blob/main/src/main/java/com/team254/lib/geometry/Pose2d.java
      Parameters:
      transform - Pose to derive velocity from
      Returns:
      Twist2d representing transformation

    • cppMod

      public static double cppMod(double num, double denom)
      Returns a modulo result that matches the C++ rather than Java implementation
      Parameters:
      num - Numerator
      denom - Denominator
      Returns:
      modulo result

    • clamp

      public static double clamp(double target, double min, double max)
      Clamps a target value between the minimum and maximum values, ensuring the value is in the given range.
      Parameters:
      target - target value
      min - min value
      max - max value
      Returns:
      the target value rounded to be in range (min, max)

    • inTolerance

      public static boolean inTolerance(double position, double lowerBound, double upperBound)
      Units must be consistent between all 3 values
      Parameters:
      position - actual position of the mechanism
      lowerBound - minimum position of the mechanism
      upperBound - maximum position of the mechanism
      Returns:
      True if position is greater than or equal to the lowerBound and less than or equal to the upperBound. False otherwise, meaning the position falls out of the range