Examples with Path com.team254.lib_2014.trajectory.Path used on opensource projects

Example 1 with Path

use of com.team254.lib_2014.trajectory.Path 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 Path

use of com.team254.lib_2014.trajectory.Path 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 3 with Path

use of com.team254.lib_2014.trajectory.Path in project Relic_Main by TeamOverdrive.

the class TextFileDeserializer method deserialize.

public Path deserialize(String serialized) {
    StringTokenizer tokenizer = new StringTokenizer(serialized, "\n");
    System.out.println("Parsing path string...");
    System.out.println("String has " + serialized.length() + " chars");
    System.out.println("Found " + tokenizer.countTokens() + " tokens");
    String name = tokenizer.nextToken();
    int num_elements = Integer.parseInt(tokenizer.nextToken());
    Trajectory left = new Trajectory(num_elements);
    for (int i = 0; i < num_elements; ++i) {
        Trajectory.Segment segment = new Trajectory.Segment();
        StringTokenizer line_tokenizer = new StringTokenizer(tokenizer.nextToken(), " ");
        segment.pos = Double.parseDouble(line_tokenizer.nextToken());
        segment.vel = Double.parseDouble(line_tokenizer.nextToken());
        segment.acc = Double.parseDouble(line_tokenizer.nextToken());
        segment.jerk = Double.parseDouble(line_tokenizer.nextToken());
        segment.heading = Double.parseDouble(line_tokenizer.nextToken());
        segment.dt = Double.parseDouble(line_tokenizer.nextToken());
        segment.x = Double.parseDouble(line_tokenizer.nextToken());
        segment.y = Double.parseDouble(line_tokenizer.nextToken());
        left.setSegment(i, segment);
    }
    Trajectory right = new Trajectory(num_elements);
    for (int i = 0; i < num_elements; ++i) {
        Trajectory.Segment segment = new Trajectory.Segment();
        StringTokenizer line_tokenizer = new StringTokenizer(tokenizer.nextToken(), " ");
        segment.pos = Double.parseDouble(line_tokenizer.nextToken());
        segment.vel = Double.parseDouble(line_tokenizer.nextToken());
        segment.acc = Double.parseDouble(line_tokenizer.nextToken());
        segment.jerk = Double.parseDouble(line_tokenizer.nextToken());
        segment.heading = Double.parseDouble(line_tokenizer.nextToken());
        segment.dt = Double.parseDouble(line_tokenizer.nextToken());
        segment.x = Double.parseDouble(line_tokenizer.nextToken());
        segment.y = Double.parseDouble(line_tokenizer.nextToken());
        right.setSegment(i, segment);
    }
    System.out.println("...finished parsing path from string.");
    return new Path(name, new Trajectory.Pair(left, right));
}

Example 4 with Path

use of com.team254.lib_2014.trajectory.Path in project Relic_Main by TeamOverdrive.

the class TestingPath method main.

public static void main(String[] args) {
    TrajectoryGenerator.Config config = new TrajectoryGenerator.Config();
    config.dt = .01;
    config.max_acc = 0.6;
    config.max_jerk = 0.4;
    config.max_vel = 2.0;
    final double kWheelbaseWidth = 12.625 / 12;
    // Path name must be a valid Java class name.
    final String path_name = "RedCloseCryptobox";
    // Description of this auto mode path.
    // Remember that this is for the GO LEFT CASE!
    WaypointSequence p = new WaypointSequence(10);
    p.addWaypoint(new WaypointSequence.Waypoint(0, 0, 0));
    p.addWaypoint(new WaypointSequence.Waypoint(7.0, 0, 0));
    Path path = PathGenerator.makePath(p, config, kWheelbaseWidth, path_name);
}

Example 5 with Path

use of com.team254.lib_2014.trajectory.Path in project Relic_Main by TeamOverdrive.

the class Arc method calculateAngleRadius.

public static Path calculateAngleRadius(TrajectoryGenerator.Config config, TrajectoryGenerator.Strategy strategy, double start_velocity, double start_heading, double radius, double goal_velocity, double goal_heading) {
    double length = Math.abs(2 * Math.PI * radius * (goal_heading - start_heading / 360));
    Path store = Arc.calculate(config, strategy, start_velocity, start_heading, length, goal_velocity, goal_heading);
    if (radius < 0)
        InvertY.calculate(store);
    return store;
}