Examples with Rotation2d com.team254.lib.geometry.Rotation2d used on opensource projects

Example 1 with Rotation2d

use of com.team254.lib.geometry.Rotation2d in project K-9 by TheGreenMachine.

the class AdaptivePurePursuitController method getLength.

public static double getLength(Pose2d pose, Translation2d point, Translation2d center, double radius) {
    if (radius < kReallyBigNumber) {
        final Translation2d centerToPoint = new Translation2d(center, point);
        final Translation2d centerToPose = new Translation2d(center, pose.getTranslation());
        // If the point is behind pose, we want the opposite of this angle. To determine if the point is behind,
        // check the sign of the cross-product between the normal vector and the vector from pose to point.
        final boolean behind = Math.signum(Translation2d.cross(pose.getRotation().normal().toTranslation(), new Translation2d(pose.getTranslation(), point))) > 0.0;
        final Rotation2d angle = Translation2d.getAngle(centerToPose, centerToPoint);
        return radius * (behind ? 2.0 * Math.PI - Math.abs(angle.getRadians()) : Math.abs(angle.getRadians()));
    } else {
        return new Translation2d(pose.getTranslation(), point).norm();
    }
}

Example 2 with Rotation2d

use of com.team254.lib.geometry.Rotation2d in project K-9 by TheGreenMachine.

the class GoalTrack method smooth.

/**
 * Averages out the observed positions based on an set of observed positions
 */
synchronized void smooth() {
    if (isAlive()) {
        double x = 0;
        double y = 0;
        // sin of angle
        double s = 0;
        // cos of angle
        double c = 0;
        double t_now = Timer.getFPGATimestamp();
        int num_samples = 0;
        for (Map.Entry<Double, Pose2d> entry : mObservedPositions.entrySet()) {
            if (t_now - entry.getKey() > Constants.kMaxGoalTrackSmoothingTime) {
                continue;
            }
            ++num_samples;
            x += entry.getValue().getTranslation().x();
            y += entry.getValue().getTranslation().y();
            c += entry.getValue().getRotation().cos();
            s += entry.getValue().getRotation().sin();
        }
        x /= num_samples;
        y /= num_samples;
        s /= num_samples;
        c /= num_samples;
        if (num_samples == 0) {
            // Handle the case that all samples are older than kMaxGoalTrackSmoothingTime.
            mSmoothedPosition = mObservedPositions.lastEntry().getValue();
        } else {
            mSmoothedPosition = new Pose2d(x, y, new Rotation2d(c, s, true));
        }
    }
}

Example 3 with Rotation2d

use of com.team254.lib.geometry.Rotation2d in project K-9 by TheGreenMachine.

the class AdaptivePurePursuitController method getCenter.

/**
 * Gives the center of the circle joining the lookahead point and robot pose
 *
 * @param pose  robot pose
 * @param point lookahead point
 * @return center of the circle joining the lookahead point and robot pose
 */
public static Translation2d getCenter(Pose2d pose, Translation2d point) {
    final Translation2d poseToPointHalfway = pose.getTranslation().interpolate(point, 0.5);
    final Rotation2d normal = pose.getTranslation().inverse().translateBy(poseToPointHalfway).direction().normal();
    final Pose2d perpendicularBisector = new Pose2d(poseToPointHalfway, normal);
    final Pose2d normalFromPose = new Pose2d(pose.getTranslation(), pose.getRotation().normal());
    if (normalFromPose.isColinear(perpendicularBisector.normal())) {
        // Special case: center is poseToPointHalfway.
        return poseToPointHalfway;
    }
    return normalFromPose.intersection(perpendicularBisector);
}