Examples with DifferentialDriveKinematics edu.wpi.first.math.kinematics.DifferentialDriveKinematics used on opensource projects

Example 1 with DifferentialDriveKinematics

use of edu.wpi.first.math.kinematics.DifferentialDriveKinematics in project 2022-RapidReact by Spartronics4915.

the class DrivetrainEstimatorTest method testEstimator.

@Test
public void testEstimator() {
    var stateStdDevs = new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.02, 0.02, 0.01);
    var measurementStdDevs = new MatBuilder<>(Nat.N6(), Nat.N1()).fill(0.1, 0.1, 0.1, 0.05, 0.05, 0.002);
    var est = new DrivetrainEstimator(stateStdDevs, measurementStdDevs, 3, new Pose2d());
    final double dt = 0.01;
    final double visionUpdateRate = 0.2;
    var traj = TrajectoryGenerator.generateTrajectory(List.of(new Pose2d(), new Pose2d(3, 3, new Rotation2d())), new TrajectoryConfig(Units.inchesToMeters(12), Units.inchesToMeters(12)));
    var kinematics = new DifferentialDriveKinematics(1);
    Pose2d lastPose = null;
    List<Double> trajXs = new ArrayList<>();
    List<Double> trajYs = new ArrayList<>();
    List<Double> observerXs = new ArrayList<>();
    List<Double> observerYs = new ArrayList<>();
    List<Double> slamXs = new ArrayList<>();
    List<Double> slamYs = new ArrayList<>();
    List<Double> visionXs = new ArrayList<>();
    List<Double> visionYs = new ArrayList<>();
    var rand = new Random();
    final double steadyStateErrorX = 1.0;
    final double steadyStateErrorY = 1.0;
    double t = 0.0;
    Pose2d lastVisionUpdate = null;
    double lastVisionUpdateT = Double.NEGATIVE_INFINITY;
    double maxError = Double.NEGATIVE_INFINITY;
    double errorSum = 0;
    while (t <= traj.getTotalTimeSeconds()) {
        t += dt;
        var groundtruthState = traj.sample(t);
        var input = kinematics.toWheelSpeeds(new ChassisSpeeds(groundtruthState.velocityMetersPerSecond, 0.0, // ds/dt * dtheta/ds = dtheta/dt
        groundtruthState.velocityMetersPerSecond * groundtruthState.curvatureRadPerMeter));
        Matrix<N3, N1> u = new MatBuilder<>(Nat.N3(), Nat.N1()).fill(input.leftMetersPerSecond * dt, input.rightMetersPerSecond * dt, 0.0);
        if (lastPose != null) {
            u.set(2, 0, groundtruthState.poseMeters.getRotation().getRadians() - lastPose.getRotation().getRadians());
        }
        u = u.plus(StateSpaceUtil.makeWhiteNoiseVector(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.002, 0.002, 0.001)));
        lastPose = groundtruthState.poseMeters;
        Pose2d realPose = groundtruthState.poseMeters;
        if (lastVisionUpdateT + visionUpdateRate < t) {
            if (lastVisionUpdate != null) {
                est.addVisionMeasurement(lastVisionUpdate, lastVisionUpdateT);
            }
            lastVisionUpdateT = t;
            lastVisionUpdate = realPose.transformBy(new Transform2d(new Translation2d(rand.nextGaussian() * 0.05, rand.nextGaussian() * 0.05), new Rotation2d(rand.nextGaussian() * 0.002)));
            visionXs.add(lastVisionUpdate.getTranslation().getX());
            visionYs.add(lastVisionUpdate.getTranslation().getY());
        }
        double dist = realPose.getTranslation().getDistance(new Translation2d());
        Pose2d measurementVSlam = realPose.transformBy(new Transform2d(new Translation2d(steadyStateErrorX * (dist / 76.0), steadyStateErrorY * (dist / 76.0)), new Rotation2d())).transformBy(new Transform2d(new Translation2d(rand.nextGaussian() * 0.05, rand.nextGaussian() * 0.05), new Rotation2d(rand.nextGaussian() * 0.001)));
        var xHat = est.update(measurementVSlam, u.get(0, 0), u.get(1, 0), u.get(2, 0), t);
        double error = groundtruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
        if (error > maxError) {
            maxError = error;
        }
        errorSum += error;
        trajXs.add(groundtruthState.poseMeters.getTranslation().getX());
        trajYs.add(groundtruthState.poseMeters.getTranslation().getY());
        observerXs.add(xHat.getTranslation().getX());
        observerYs.add(xHat.getTranslation().getY());
        slamXs.add(measurementVSlam.getTranslation().getX());
        slamYs.add(measurementVSlam.getTranslation().getY());
    }
    System.out.println("Mean error (meters): " + errorSum / (traj.getTotalTimeSeconds() / dt));
    System.out.println("Max error (meters):  " + maxError);
    try {
        if (true)
            throw new HeadlessException();
        var chartBuilder = new XYChartBuilder();
        chartBuilder.title = "The Magic of Sensor Fusion";
        var chart = chartBuilder.build();
        chart.addSeries("vSLAM", slamXs, slamYs);
        chart.addSeries("Vision", visionXs, visionYs);
        chart.addSeries("Trajectory", trajXs, trajYs);
        chart.addSeries("xHat", observerXs, observerYs);
        new SwingWrapper<>(chart).displayChart();
        try {
            Thread.sleep(1000000000);
        } catch (InterruptedException e) {
        }
    } catch (java.awt.HeadlessException ex) {
        System.out.println("skipping charts in headless mode");
    }
}

Example 2 with DifferentialDriveKinematics

use of edu.wpi.first.math.kinematics.DifferentialDriveKinematics in project NerdyLib by nerdherd.

the class Drivetrain method configKinematics.

public void configKinematics(double trackwidth, Rotation2d startingAngle, Pose2d startingPose) {
    m_kinematics = new DifferentialDriveKinematics(trackwidth);
    m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getRawYaw()));
}

Example 3 with DifferentialDriveKinematics

use of edu.wpi.first.math.kinematics.DifferentialDriveKinematics in project RapidReact2022 by team223.

the class DrivePath method initialize.

// Called when the command is initially scheduled.
@Override
public void initialize() {
    drivePid = new PIDController(0.5, 0.3, 0);
    try {
        Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
        trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
    } catch (IOException ex) {
        System.out.println("Unable to open trajectory: " + trajectoryJSON);
    }
    kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(23.5));
    startTimeMillis = System.currentTimeMillis();
    System.out.println("start!");
}

Example 4 with DifferentialDriveKinematics

use of edu.wpi.first.math.kinematics.DifferentialDriveKinematics in project PathWeaver by wpilibsuite.

the class WpilibSpline method writeToFile.

@Override
public boolean writeToFile(java.nio.file.Path path) {
    final AtomicBoolean okay = new AtomicBoolean(true);
    TrajectoryGenerator.setErrorHandler((error, stacktrace) -> {
        LOGGER.log(Level.WARNING, "Could not write Spline to file: " + error, stacktrace);
        okay.set(false);
    });
    try {
        var values = ProjectPreferences.getInstance().getValues();
        var prefs = ProjectPreferences.getInstance();
        var lengthUnit = prefs.getField().getUnit();
        double height = prefs.getField().getRealLength().getValue().doubleValue();
        var maxVelocity = values.getMaxVelocity();
        var maxAcceleration = values.getMaxAcceleration();
        var trackWidth = values.getTrackWidth();
        // If the export type is different (i.e. meters), then we have to convert it. Otherwise we are good.
        if (prefs.getValues().getExportUnit() == ProjectPreferences.ExportUnit.METER) {
            UnitConverter converter = lengthUnit.getConverterTo(PathUnits.METER);
            height = converter.convert(height);
            maxVelocity = converter.convert(maxVelocity);
            maxAcceleration = converter.convert(maxAcceleration);
            trackWidth = converter.convert(trackWidth);
        }
        TrajectoryConfig config = new TrajectoryConfig(maxVelocity, maxAcceleration).setKinematics(new DifferentialDriveKinematics(trackWidth)).setReversed(waypoints.get(0).isReversed());
        Trajectory traj = trajectoryFromWaypoints(waypoints, config);
        for (int i = 0; i < traj.getStates().size(); ++i) {
            var st = traj.getStates().get(i);
            traj.getStates().set(i, new Trajectory.State(st.timeSeconds, st.velocityMetersPerSecond, st.accelerationMetersPerSecondSq, new Pose2d(st.poseMeters.getX(), st.poseMeters.getY() + height, st.poseMeters.getRotation()), st.curvatureRadPerMeter));
        }
        TrajectoryUtil.toPathweaverJson(traj, path.resolveSibling(path.getFileName() + ".wpilib.json"));
        return okay.get();
    } catch (IOException except) {
        LOGGER.log(Level.WARNING, "Could not write Spline to file", except);
        return false;
    }
}

Example 5 with DifferentialDriveKinematics

use of edu.wpi.first.math.kinematics.DifferentialDriveKinematics in project RapidReact2022 by team223.

the class AutoDrive1 method initialize.

// Called when the command is initially scheduled.
@Override
public void initialize() {
    drivePid = new PIDFController(.3, 0.02, 0, 0.03);
    try {
        Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
        trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
    } catch (IOException ex) {
        System.out.println("Unable to open trajectory: " + trajectoryJSON);
    }
    kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(23.5));
    startTimeMillis = System.currentTimeMillis();
    System.out.println("start!");
}