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

Example 1 with Pose2d

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

the class AdaptivePurePursuitController method update.

/**
 * Gives the Pose2d.Delta that the robot should take to follow the path
 *
 * @param pose robot pose
 * @return movement command for the robot to follow
 */
public Command update(Pose2d pose) {
    if (mReversed) {
        pose = new Pose2d(pose.getTranslation(), pose.getRotation().rotateBy(Rotation2d.fromRadians(Math.PI)));
    }
    final Path.TargetPointReport report = mPath.getTargetPoint(pose.getTranslation(), mLookahead);
    if (isFinished()) {
        // Stop.
        return new Command(Twist2d.identity(), report.closest_point_distance, report.max_speed, 0.0, report.lookahead_point, report.remaining_path_distance);
    }
    final Arc arc = new Arc(pose, report.lookahead_point);
    double scale_factor = 1.0;
    // Ensure we don't overshoot the end of the path (once the lookahead speed drops to zero).
    if (report.lookahead_point_speed < 1E-6 && report.remaining_path_distance < arc.length) {
        scale_factor = Math.max(0.0, report.remaining_path_distance / arc.length);
        mAtEndOfPath = true;
    } else {
        mAtEndOfPath = false;
    }
    if (mReversed) {
        scale_factor *= -1;
    }
    return new Command(new Twist2d(scale_factor * arc.length, 0.0, arc.length * getDirection(pose, report.lookahead_point) * Math.abs(scale_factor) / arc.radius), report.closest_point_distance, report.max_speed, report.lookahead_point_speed * Math.signum(scale_factor), report.lookahead_point, report.remaining_path_distance);
}

Example 2 with Pose2d

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

the class AdaptivePurePursuitController method getLength.

/**
 * Gives the length of the arc joining the lookahead point and robot pose (assuming forward motion).
 *
 * @param pose  robot pose
 * @param point lookahead point
 * @return the length of the arc joining the lookahead point and robot pose
 */
public static double getLength(Pose2d pose, Translation2d point) {
    final double radius = getRadius(pose, point);
    final Translation2d center = getCenter(pose, point);
    return getLength(pose, point, center, radius);
}

Example 3 with Pose2d

use of com.team254.lib.geometry.Pose2d 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 4 with Pose2d

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

the class PathFollower method update.

/**
 * Get new velocity commands to follow the path.
 *
 * @param t            The current timestamp
 * @param pose         The current robot pose
 * @param displacement The current robot displacement (total distance driven).
 * @param velocity     The current robot velocity.
 * @return The velocity command to apply
 */
public synchronized Twist2d update(double t, Pose2d pose, double displacement, double velocity) {
    if (!mSteeringController.isFinished()) {
        final AdaptivePurePursuitController.Command steering_command = mSteeringController.update(pose);
        mDebugOutput.lookahead_point_x = steering_command.lookahead_point.x();
        mDebugOutput.lookahead_point_y = steering_command.lookahead_point.y();
        mDebugOutput.lookahead_point_velocity = steering_command.end_velocity;
        mDebugOutput.steering_command_dx = steering_command.delta.dx;
        mDebugOutput.steering_command_dy = steering_command.delta.dy;
        mDebugOutput.steering_command_dtheta = steering_command.delta.dtheta;
        mCrossTrackError = steering_command.cross_track_error;
        mLastSteeringDelta = steering_command.delta;
        mVelocityController.setGoalAndConstraints(new MotionProfileGoal(displacement + steering_command.delta.dx, Math.abs(steering_command.end_velocity), CompletionBehavior.VIOLATE_MAX_ACCEL, mGoalPosTolerance, mGoalVelTolerance), new MotionProfileConstraints(Math.min(mMaxProfileVel, steering_command.max_velocity), mMaxProfileAcc));
        if (steering_command.remaining_path_length < mStopSteeringDistance) {
            doneSteering = true;
        }
    }
    final double velocity_command = mVelocityController.update(new MotionState(t, displacement, velocity, 0.0), t);
    mAlongTrackError = mVelocityController.getPosError();
    final double curvature = mLastSteeringDelta.dtheta / mLastSteeringDelta.dx;
    double dtheta = mLastSteeringDelta.dtheta;
    if (!Double.isNaN(curvature) && Math.abs(curvature) < kReallyBigNumber) {
        // Regenerate angular velocity command from adjusted curvature.
        final double abs_velocity_setpoint = Math.abs(mVelocityController.getSetpoint().vel());
        dtheta = mLastSteeringDelta.dx * curvature * (1.0 + mInertiaGain * abs_velocity_setpoint);
    }
    final double scale = velocity_command / mLastSteeringDelta.dx;
    final Twist2d rv = new Twist2d(mLastSteeringDelta.dx * scale, 0.0, dtheta * scale);
    // Fill out debug.
    mDebugOutput.t = t;
    mDebugOutput.pose_x = pose.getTranslation().x();
    mDebugOutput.pose_y = pose.getTranslation().y();
    mDebugOutput.pose_theta = pose.getRotation().getRadians();
    mDebugOutput.linear_displacement = displacement;
    mDebugOutput.linear_velocity = velocity;
    mDebugOutput.profile_displacement = mVelocityController.getSetpoint().pos();
    mDebugOutput.profile_velocity = mVelocityController.getSetpoint().vel();
    mDebugOutput.velocity_command_dx = rv.dx;
    mDebugOutput.velocity_command_dy = rv.dy;
    mDebugOutput.velocity_command_dtheta = rv.dtheta;
    mDebugOutput.cross_track_error = mCrossTrackError;
    mDebugOutput.along_track_error = mAlongTrackError;
    return rv;
}

Example 5 with Pose2d

use of com.team254.lib.geometry.Pose2d 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));
        }
    }
}