TrapezoidalMotionProfile updating not working correctly
tylertian123 opened this issue · comments
When using dynamic motion profile re-generation with TrapezoidalTankDriveProfiles, an IllegalArgumentException is thrown. Full stack trace:
[112.38] ERROR 1 Unhandled exception: java.lang.IllegalArgumentException: Time out of range (0.786478 ? [113.037009, 2.430556])!
com.arctos6135.robotpathfinder.motionprofile.TrapezoidalMotionProfile.position(TrapezoidalMotionProfile.java:169)
at com.arctos6135.robotpathfinder.motionprofile.TrapezoidalMotionProfile.position(TrapezoidalMotionProfile.java:169)
at com.arctos6135.robotpathfinder.motionprofile.DualMotionProfile.leftPosition(DualMotionProfile.java:68)
at com.arctos6135.robotpathfinder.motionprofile.followable.TankDriveFollowableMotionProfile.get(TankDriveFollowableMotionProfile.java:40)
at com.arctos6135.robotpathfinder.motionprofile.followable.TankDriveFollowableMotionProfile.get(TankDriveFollowableMotionProfile.java:21)
at com.arctos6135.robotpathfinder.follower.DynamicTankDriveFollower._run(DynamicTankDriveFollower.java:482)
at com.arctos6135.robotpathfinder.follower.Follower.run(Follower.java:248)
at com.arctos6135.robotpathfinder.follower.DynamicFollower.run(DynamicFollower.java:74)
at frc.robot.commands.FollowTrajectory.execute(FollowTrajectory.java:175)
at frc.robot.commands.DriveDistance.execute(DriveDistance.java:46)
at edu.wpi.first.wpilibj.command.Command.run(Command.java:292)
at edu.wpi.first.wpilibj.command.CommandGroup._execute(CommandGroup.java:234)
at edu.wpi.first.wpilibj.command.Command.run(Command.java:291)
at edu.wpi.first.wpilibj.command.Scheduler.run(Scheduler.java:224)
at frc.robot.Robot.teleopPeriodic(Robot.java:427)
at edu.wpi.first.wpilibj.IterativeRobotBase.loopFunc(IterativeRobotBase.java:240)
at edu.wpi.first.wpilibj.TimedRobot.startCompetition(TimedRobot.java:81)
at edu.wpi.first.wpilibj.RobotBase.startRobot(RobotBase.java:263)
at frc.robot.Main.main(Main.java:27)
Some additional logs from the robot:
robot0.log
robot1.log
robot2.log
robot3.log
From robot0.log with the weird distance and time values passed to update() one can determine that part of the bug was caused by DynamicTankDriveFollower._update() not passing the correct parameters. But even after this problem has been fixed, the crashes still happen.
Some more logs:
robot4.log
robot5.log
robot6.log
robot7.log
robot8.log
Some of these logs may contain more information than others. robot7.log and robot8.log should be the logs from the final version of the code from the day's testing. The updated code will be pushed soon.
The new code has been pushed. It is unknown whether or not TrapezoidalMotionProfiles work with negative distances and updating at this point. Additionally, it seems like the update() method will throw an IllegalArgumentException if it is called with a velocity that's greater than the maximum velocity used to initially construct the TrapezoidalMotionProfile. However, this may be undesirable at times because in real life there will be measurement errors, and we don't want the code to crash because of that. Finally, real-life testing shows that in many cases, updating will cause the robot to overshoot, after which there will be an aggressive corrective behaviour, which is undesirable. This can be seen in robot8.log.
Two more log files:
robot9.log
robot10.log
Note that these contain ANSI escape codes for highlighting to make the log files easier to read.
It seems like the problems with IllegalArgumentExceptions should be fixed now, but the underlying math in TrapezoidalMotionProfiles may be wrong.
IllegalArgumentExceptions are still happening?
bug.log
The last few commits pushed did significantly improve the accuracy of dynamic TrapezoidalMotionProfiles though. Now the distance is always fairly accurate. Here's some data collected on Monday, Nov 25:
data.log
data1.log
data2.log
data3.log
data4.log
data5.log
data6.log
These log files all contain a bunch of data about the expected vs actual motion of the robot at a given time. Use this to graph it:
public class GraphData {
public static void main(String[] args) throws FileNotFoundException, IOException {
BufferedReader reader = new BufferedReader(new FileReader(new File("data6.log")));
String line;
List<Double> t = new ArrayList<>();
List<Double> pe = new ArrayList<>();
List<Double> ve = new ArrayList<>();
List<Double> pa = new ArrayList<>();
List<Double> va = new ArrayList<>();
System.out.println("Collecting Data");
while((line = reader.readLine()) != null) {
Map<String, Double> data = new HashMap<>();
for(String s : line.split(",")) {
String[] d = s.split("=");
data.put(d[0], Double.parseDouble(d[1]));
}
t.add(data.get("t"));
pe.add(data.get("lpe"));
ve.add(data.get("lve"));
pa.add(data.get("lpa"));
va.add(data.get("lva"));
}
Double[] t1 = new Double[t.size()];
t.toArray(t1);
Double[] pe1 = new Double[pe.size()];
pe.toArray(pe1);
Double[] ve1 = new Double[ve.size()];
ve.toArray(ve1);
Double[] pa1 = new Double[pa.size()];
pa.toArray(pa1);
Double[] va1 = new Double[va.size()];
va.toArray(va1);
System.out.println("Graphing");
Runnable r = () -> {
// Create plot
Plot2DPanel plot = new Plot2DPanel();
plot.setLegendOrientation("EAST");
// Add graphs
double[] time = Arrays.stream(t1).mapToDouble(d -> d).toArray();
plot.addLinePlot("Expected Position", time, Arrays.stream(pe1).mapToDouble(d -> d).toArray());
plot.addLinePlot("Expected Velocity", time, Arrays.stream(ve1).mapToDouble(d -> d).toArray());
plot.addLinePlot("Actual Position", time, Arrays.stream(pa1).mapToDouble(d -> d).toArray());
plot.addLinePlot("Actual Velocity", time, Arrays.stream(va1).mapToDouble(d -> d).toArray());
// Create window that holds the graph
JFrame frame = new JFrame("Trajectory Graph");
frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
frame.setContentPane(plot);
// Maximize
frame.setExtendedState(JFrame.MAXIMIZED_BOTH);
frame.setVisible(true);
};
SwingUtilities.invokeLater(r);
reader.close();
}
}One strange thing that seemed to be happening in these tests is that sometimes there will be virtually no acceleration for the first 0.3s or so. It seems like there's no data for this period, which means that the control loop was not called (strange).
The IllegalArgumentExceptions were caused by other code in the follow trajectory command calling position() on the motion profile and is not actually a bug. The bad error messages are caused by an unrelated bug and have been fixed.
Some more final testing is needed, but it seems like for the most part this issue should be fixed.
Should be fixed for now... Will be reopened in the future if the problem still exists.