use of edu.wpi.first.math.trajectory.TrajectoryConfig in project 6591-2022 by jafillmore.
the class RobotContainer method getAutonomousCommand.
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// Create config for trajectory
TrajectoryConfig config = new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond, AutoConstants.kMaxAccelerationMetersPerSecondSquared).setKinematics(DriveConstants.kDriveKinematics);
// An example trajectory to follow. All units in meters.
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(// Start at the origin facing the +X direction
new Pose2d(0, 0, new Rotation2d(0)), // Pass through these two interior waypoints, making an 's' curve path
List.of(new Translation2d(1, 1), new Translation2d(2, -1)), // End 3 meters straight ahead of where we started, facing forward
new Pose2d(3, 0, new Rotation2d(0)), config);
MecanumControllerCommand mecanumControllerCommand = new MecanumControllerCommand(exampleTrajectory, m_robotDrive::getPose, DriveConstants.kFeedforward, DriveConstants.kDriveKinematics, // Position contollers
new PIDController(AutoConstants.kPXController, 0, 0), new PIDController(AutoConstants.kPYController, 0, 0), new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints), // Needed for normalizing wheel speeds
AutoConstants.kMaxSpeedMetersPerSecond, // Velocity PID's
new PIDController(DriveConstants.kPFrontLeftVel, 0, 0), new PIDController(DriveConstants.kPRearLeftVel, 0, 0), new PIDController(DriveConstants.kPFrontRightVel, 0, 0), new PIDController(DriveConstants.kPRearRightVel, 0, 0), m_robotDrive::getCurrentWheelSpeeds, // Consumer for the output motor voltages
m_robotDrive::setDriveMotorControllersVolts, m_robotDrive);
// Reset odometry to the starting pose of the trajectory.
m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
// Run path following command, then stop at the end.
return mecanumControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
}
use of edu.wpi.first.math.trajectory.TrajectoryConfig in project RapidReact2022 by robototes.
the class AutonomousCommand method getAutonomousCommand.
public Command getAutonomousCommand() {
// Create config for trajectory
TrajectoryConfig config = new TrajectoryConfig(AutoConstants.MAX_SPEED_METERS_PER_SECOND, AutoConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED).setKinematics(AutoConstants.driveKinematics);
// creating trajectory path (right now is a square)
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(new Pose2d(0, 0, Rotation2d.fromDegrees(0)), List.of(new Translation2d(1, 0), new Translation2d(1, 1), new Translation2d(0, 1)), new Pose2d(0, 0.0, Rotation2d.fromDegrees(0)), config);
// creates thetacontroller (rotation)
ProfiledPIDController thetaController = new ProfiledPIDController(0.000000005, 0, 0, AutoConstants.K_THETA_CONTROLLER_CONSTRAINTS);
thetaController.enableContinuousInput(-Math.PI, Math.PI);
exampleTrajectory.relativeTo(drivebaseSubsystem.getPoseAsPoseMeters());
SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(exampleTrajectory, // Functional interface to feed supplier
drivebaseSubsystem::getPoseAsPoseMeters, AutoConstants.driveKinematics, // Position controllers
new PIDController(AutoConstants.PX_CONTROLLER, 0, 0), new PIDController(AutoConstants.PY_CONTROLLER, 0, 0), thetaController, drivebaseSubsystem::updateModules, drivebaseSubsystem);
return swerveControllerCommand;
}
use of edu.wpi.first.math.trajectory.TrajectoryConfig in project FRC2022 by 2202Programming.
the class getTrajectoryFollowTest method initialize.
// Called when the command is initially scheduled.
@Override
public void initialize() {
// An example trajectory to follow. All units in feet.
Rotation2d current_angle = new Rotation2d(sensors.getYaw());
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(new Pose2d(0.0, 0.0, current_angle), List.of(), new Pose2d(0, 3.0, current_angle), // max velocity, max accel
new TrajectoryConfig(2.0, 0.5));
SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(exampleTrajectory, // Functional interface to feed supplier
drivetrain::getPose, drivetrain.getKinematics(), // Position controllers
new PIDController(4.0, 0.0, 0.0), new PIDController(4.0, 0.0, 0.0), new ProfiledPIDController(4, 0, 0, new TrapezoidProfile.Constraints(.3, .3)), // per second squared
drivetrain::drive, drivetrain);
// Reset odometry to the starting pose of the trajectory.
drivetrain.setPose(exampleTrajectory.getInitialPose());
// Run path following command, then stop at the end.
work = swerveControllerCommand.andThen(() -> drivetrain.stop()).withTimeout(10);
}
use of edu.wpi.first.math.trajectory.TrajectoryConfig 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");
}
}
use of edu.wpi.first.math.trajectory.TrajectoryConfig 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;
}
}
Aggregations